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ABSTRACT 


Title  of  Dissertation:  GEOMETRIC  COOPERATIVE  CONTROL  OF 

FORMATIONS 

Fumin  Zhang,  Doctor  of  Philosophy,  2004 


Dissertation  directed  by:  Professor  P.S.  Krishnaprasad 

Department  of  Electrical  &  Computer  Engineering 


Robots  in  a  team  are  modeled  as  particles  which  obey  simple,  second 
order  dynamics.  The  whole  team  can  be  viewed  as  a  deformable  body  with 
changing  shape  and  orientation.  Jacobi  shape  theory  is  applied  to  model  such 
a  formation.  The  configuration  space  of  the  whole  formation  is  then  viewed 
as  a  fiber  bundle  on  Jacobi  shape  space.  Applications  of  shape  theory  to 
analyze  problems,  such  as  the  sequential  pursuit,  provide  insights  on  body 
motion  and  shape  changes. 

We  derive  the  controlled  system  equations  using  the  Lagrange- D  ’Alembert 
principle.  In  the  resulting  Lagrangian  equations,  the  motion  of  the  center  of 
mass  of  the  formation  is  decoupled  from  the  rotation  and  shape  dynamics. 
Furthermore,  control  forces  on  each  robot  are  combined  and  reorganized  as 


controls  for  the  center,  for  rotation  and  for  shape  changes.  From  a  shape- 
theoretic  point  of  view,  general  feedback  control  laws  are  designed  to  achieve 
desired  formations.  This  class  of  feedback  control  laws  induces  interactions 
that  can  be  computed  in  a  distributed  manner.  When  communication  links 
and  GPS  are  available  to  each  robot,  we  use  Jacobi  vectors  as  feedback  for 
control.  We  show  that  formations  are  set  up  asymptotically. 

The  system  equations  on  shape  space  provide  possibilities  for  achieving 
formations  without  communication  links  between  team  members,  under  the 
assumption  that  each  robot  uses  its  sensors  to  measure  shape  and  rotation 
of  the  formation.  We  allow  each  robot  freedom  to  establish  a  coordinate  sys¬ 
tem  in  which  shape  dynamics  of  the  whole  formation  is  computed.  Without 
knowing  such  coordinate  systems  of  other  robots,  each  robot  is  able  to  per¬ 
form  cooperative  control.  This  is  made  possible  by  a  class  of  gauge  covariant 
control  laws.  Using  Lyapunov  functions,  we  prove  that  controlled  dynamics 
converges  to  an  invariant  set  where  desired  shape  is  achieved.  We  argue  that 
this  freedom  of  choosing  gauge  frame  helps  to  improve  controller  performance 
in  a  noisy  environment. 

When  all  robots  are  required  to  have  common  constant  speed,  the  con¬ 
trol  forces  have  to  be  of  gyroscopic  nature.  Previous  works  of  Justh  and 
Krishnaprasad  has  proposed  a  shape  theory  in  this  context,  together  with  a 
class  of  gyroscopic  control  laws  that  are  able  to  produce  team  behavior.  This 
has  inspired  us  to  study  the  obstacle  avoidance  and  navigation  problem  of 
a  constant  speed  particle  from  a  point  of  view  of  formation  shape  control. 


We  achieve  gyroscopic  control  laws  to  produce  suitable  interaction  between 
a  moving  particle  and  a  piecewise  smooth  curve.  The  particular  goal  is  to 
achieve  boundary  following  behavior  when  the  particle  encounters  an  obsta¬ 
cle.  The  “steady  state”  trajectory  of  the  particle  forms  a  Bertrand  pair  with 
the  boundary  curve  of  the  obstacle.  We  discover  that  this  steady  state  be¬ 
havior  correspond  to  a  relative  equilibrium  for  a  non- autonomous  system  on 
special  Euclidean  groups.  Our  control  law  achieves  asymptotic  convergence 
of  the  non-autonomous  system  dynamics. 

The  boundary  following  behavior  is  a  building  block  for  robot  navigation 
in  a  cluttered  environment.  Based  on  the  configuration  of  the  obstacles  and 
the  target,  we  may  construct  virtual  boundary  curves  by  analyzing  sensory 
information  of  the  robot.  Such  virtual  boundary  curves  lead  the  robot  to  the 
target  without  collision. 

We  also  study  the  problem  of  establishing  a  formation  of  satellites  near  an 
elliptic  earth  orbit.  Such  formations  have  periodic  shape  changes.  Therefore, 
the  shape  space  is  constructed  by  utilizing  conserved  quantities  of  motion 
instead  of  Jacobi  shape  variables.  We  propose  a  control  law  that  would 
set  up  a  given  formation  near  a  given  orbit.  This  law  also  allows  a  satellite 
formation  to  achieve  orbit  transfer.  During  the  transfer,  the  formation  can  be 
either  maintained  or  modified  to  a  desired  one.  Based  on  the  orbit  transfer 
control  law  proposed  by  Chang,  Chichka  and  Marsden  for  single  satellite, 
we  add  coupling  terms  to  the  summation  of  Lyapunov  functions  for  single 
satellites.  These  terms  are  functions  of  the  difference  between  the  mean 


anomalies  (or  perigee  passage  times)  of  formation  members.  The  asymptotic 
stability  of  the  desired  formation  in  desired  orbits  is  proved. 
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Chapter  1 


Introduction  and  Preliminaries 


In  human  societies,  formations  are  formed  deliberately  in  numerous  situations 
to  achieve  goals  beyond  individual  ability.  The  shapes  of  formations  are 
determined  according  to  target  applications.  For  example,  rectangles  are 
formed  to  transport  heavy  burdens.  Lines  are  used  for  hunting  or  attacking. 
Circles  are  used  for  defense... etc.  In  sports  and  entertainment,  keeping  and 
changing  formations  is  appreciated  as  a  form  of  art. 

Human  ability  to  achieve  and  maintain  formations  is  acquired  by  learn¬ 
ing  and  training.  The  algorithms  are  based  on  a  set  of  rules.  Commands  or 
drums  are  used  as  synchronization  mechanisms.  However,  this  type  of  rule- 
based  formation  control  requires  very  high  level  intelligence  which  will  not 
be  reached  by  any  man-made  device  in  the  near  future.  On  the  other  hand, 
many  examples  of  formations  have  been  observed  in  mammals,  birds,  fish 
and  insects  with  much  lower  level  intelligence.  The  most  popular  one  is  the 


1 


V-shape  formation  formed  by  geese  and  various  kinds  of  birds.  Researchers 
have  come  up  with  different  explanations  of  why  and  how  this  shape  is  formed 
but  the  problem  is  still  an  open  one([12]).  What  has  been  observed  is  that 
formations  will  help  animals  to  cooperate  effectively  to  face  challenges  from 
nature.  A  formation  will  provide  more  information  sharing,  more  opportuni¬ 
ties  in  reproducing,  safer  environment  and  less  consumption  of  energy.  Many 
attempts  have  been  made  in  both  explaining  and  simulating  the  behaviors  of 
animal  formations([27],[17]).  Although  the  results  are  heuristic  and  approxi¬ 
mate,  they  can  serve  as  inspirations  for  designing  and  controlling  multi-robot 
formations. 

In  physics,  the  world  without  intelligence,  various  structures  of  molecules 
and  the  structure  of  the  solar  system  all  suggest  that  formations  can  be 
achieved  by  basic  interactions  between  objects.  The  problem  of  determining 
motion  of  a  group  of  particles  with  mutual  interactions  is  called  the  many- 
body  problem.  This  problem  has  a  history  back  to  Kepler,  Newton  and  Euler 
and  is  still  a  focus  of  research  in  modern  physics.  What  are  the  interactions 
which  allow  particles  to  form  a  meaningful  formation  is  the  problem  we  want 
to  address  in  this  thesis. 

This  problem  is  not  only  interesting  theoretically,  but  also  very  much 
relevant  to  applications.  Some  recent  development  in  distributed  sensor  net¬ 
works  and  adaptive  sampling  raised  many  interests  in  formation  control  that 
is  able  to  react  to  environment  changes,  c.f.  [11]. 

One  must  keep  in  mind  that  Nature  does  not  know  how  to  determine 
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the  coordinates  of  the  particles.  The  laws  of  physics,  usually  represented  in 
formulas  involving  coordinates,  are  actually  coordinate  independent.  A  robot 
can  be  viewed  as  a  physical  object  with  low  level  intelligence.  Without  help 
from  a  fixed  coordinate  frame,  only  shape  variables  can  be  determined  using 
its  on-board  sensors.  As  students  of  Nature,  we  can  artificially  introduce 
simulated  physical  interactions  between  robots  to  achieve  formations.  This 
serves  as  the  main  theme  of  this  thesis.  We  justify  this  claim  by  suggesting 
mathematically  justified  interaction  laws  for  mobile  robots,  unmanned  aerial 
vehicles  and  man-made  satellites. 


1.1  Contributions  of  the  thesis 

Jacobi  proposed  a  special  class  of  coordinates  ([23])  which  served  as  the 
starting  point  to  what  we  call  the  Jacobi  shape  space.  On  this  shape  space, 
global  displacements  (of  translation  and  rotation)  are  not  present.  What 
remain  are  quantities  which  are  coordinate  free.  In  fact,  the  gravitational 
potential  function  and  electric  potential  function,  which  induce  the  two  basic 
natural  forces,  are  functions  on  this  Jacobi  shape  space.  It  suggests  that 
Nature  knows  how  to  measure  shapes. 

The  Jacobi  shape  space  can  be  parametrized  in  different  ways.  Working 
independently,  a  statistician  D.  G.  Kendall  developed  his  notion  of  size-shape 
space  ([15],  [16])  which  turns  out  to  be  a  specially  parametrized  Jacobi  shape 
space.  Kendall  defined  a  size  variable  which  we  prove  to  be  the  virial  function 
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well  known  in  physics.  After  the  size  information  is  removed  from  the  Jacobi 
shape  space,  one  gets  a  compact  space  of  “pure  shapes”  which  is  the  Kendall 
shape  space. 

The  above  discussions  of  shape  spaces  are  elaborated  in  chapter  2.  These 
insightful  results  provides  mathematical  tools  describing  formations.  To  show 
that  we  can  take  advantage  of  the  knowledge  of  shape  spaces.  We  give  two 
examples  at  the  end  of  chapter  2  .  In  the  first  example,  we  analyze  a  clas¬ 
sical  example  of  sequential  pursuit.  We  discovered  that  the  gauge  covariant 
angular  velocity  is  a  conserved  quantity  which  contributes  to  the  beauty  of 
system  trajectories.  The  second  example  is  controlling  line  formations  with 
possible  applications  in  automated  highway  systems.  In  these  examples,  our 
results  benefit  from  Kendall’s  notion  of  size  and  shape. 

In  this  spirit,  we  present  some  results  using  Lyapunov-based  design  meth¬ 
ods.  The  central  idea  is  to  find  a  candidate  function  with  compact  sub-level 
sets.  Then  design  a  control  law  which  produces  semi-negative  derivative  for 
this  function.  We  can  then  argue  by  using  LaSalle’s  invariance  principle  that 
the  controlled  dynamics  converges  to  the  maximal  invariant  set.  If  the  Lya¬ 
punov  candidate  function  is  a  function  on  shape  space,  then  the  feedback 
control  law  only  depends  on  shape  variables. 

In  chapter  3,  a  formation  of  Newtonian  particles  is  modeled  as  a  controlled 
Lagrangian  system.  When  there  is  symmetry  in  the  Lagrangian  function,  the 
dynamics  can  be  reduced  to  either  the  shape  space  or  the  pre-shape  space. 
Then  it  is  possible  to  design  a  control  law  based  on  a  Lyapunov  function  on 


4 


the  pre-shape  space  or  shape  space.  Theorem  3.2.1  claims  that  a  simple  con¬ 
trol  law  using  measurements  of  Jacobi  vectors  can  be  utilized  to  setup  a  given 
formation.  However,  those  measurements  require  the  real  time  availability  of 
GPS  and  high  speed  communication  links.  Theorem  3.3.1  and  3.3.2  claims 
that  a  desired  formation  can  be  set  up  by  using  measurements  of  shape  vari¬ 
ables.  Since  the  measurements  can  be  obtained  through  sensors  of  the  robot, 
no  GPS  is  required.  To  stop  the  rotation  motion  of  the  formation,  in  theorem 
3.3.1,  measurements  of  the  angular  velocity  are  employed  as  feedback.  The¬ 
orem  3.3.2  states  that  if  there  exists  certain  friction  in  the  environment  then 
the  formation  will  stop  rotation  without  using  a  controller.  The  motivation 
to  study  such  control  laws  is  to  lessen  the  demands  on  communication  links 
posed  by  formation  controllers. 

The  robots  that  form  a  formation  usually  have  reasonably  strong  com¬ 
puting  and  sensing  ability.  An  ideal  cooperative  control  law  should  be  able 
to  allow  maximum  freedom  for  each  robot  in  addition  to  achieving  coop¬ 
eration.  The  geometry  of  the  tangent  bundle  on  the  shape  space  provides 
a  class  of  gauge  invariant  and  gauge  covariant  objects.  When  measured  in 
different  coordinate  systems,  these  objects  will  either  not  change  its  value 
in  different  gauge  coordinate  frame  or  the  change  is  compatible  with  the 
frame  transform.  Theorem  3.4.1  shows  one  way  to  utilize  the  gauge  invari¬ 
ant  and  gauge  covariant  objects  to  allow  each  robot  freedom  to  establish  its 
own  choice  of  gauge  coordinate  frame.  Simulation  shows  that  by  having  this 
freedom,  noise  in  the  sensor  data  might  have  less  effect  on  the  performance 
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of  formation  controllers. 


The  construction  of  Jacobi  shape  space  inspired  us  to  look  into  the  pos¬ 
sibilities  of  constructing  other  shape  spaces  suitable  for  specific  context.  In 
applications  involving  unmanned  aerial  vehicles  (UAVs)  and  mobile  robots, 
the  configuration  space  of  each  robot  is  a  Euclidean  group  SE( 2)  or  SE( 3). 
Meanwhile,  regardless  of  its  shape,  the  whole  formation  can  be  described  by 
one  group  element  that  belongs  to  the  same  Euclidean  group.  Then  shape 
variables  are  derived  by  computing  the  relative  displacement  between  the 
group  element  of  each  member  and  the  group  element  for  the  formation. 

Due  to  fuel  efficiency  and  dynamical  stability,  UAVs  and  high  speed  mo¬ 
bile  robots  are  preferred  to  maintain  constant  speed.  With  the  constant  speed 
constraint,  the  interactions  between  the  formation  members  are  of  gyroscopic 
nature.  Several  ways  of  designing  gyroscopic  interactions  to  achieve  forma¬ 
tions  have  been  proposed  by  [14],  We  discovered  that  the  formation  control 
technique  can  be  applied  to  generate  a  control  law  for  obstacle  avoidance  and 
navigation  for  constant  speed  vehicles. 

Our  navigation  algorithms  are  based  on  a  class  of  control  laws  that  pro¬ 
duce  boundary  following  behaviors  for  moving  vehicles.  The  steady  state 
trajectory  achieved  by  this  control  law  is  a  Bertrand  mate  of  the  boundary 
curve  of  an  obstacle.  We  describe  the  pair  of  planar  Bertrand  curves  using 
the  Frenet-Serret  formulation.  Theorem  4.2.2  shows  that  if  a  Bertrand  pair 
of  curves  is  traversed  by  a  pair  of  points,  then  there  is  at  least  one  configura¬ 
tion  where  the  shape  of  the  points  remains  constant.  Theorem  4.3.1  claims 
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that  a  Bertrand  pair  can  be  viewed  as  a  relative  equilibrium  for  the  motion 
of  a  pair  of  particles.  We  discovered  that  if  one  particle  is  a  constant  speed 
vehicle,  then  the  other  particle  correspond  to  the  closest  point  to  the  vehicle 
on  the  boundary  curve.  To  make  the  vehicle  follow  the  boundary  curve,  all 
we  have  to  do  now  is  to  find  control  laws  to  achieve  this  equilibrium. 

We  have  proposed  two  such  control  laws.  The  first  control  law  assumes 
that  the  boundary  curve  is  smooth.  The  resulting  tracking  behavior  is  in¬ 
dependent  of  the  choice  for  the  direction  of  boundary  curve.  The  second 
control  law  only  assumes  piecewise  smoothness  of  the  boundary  curve.  The 
vehicle  is  controlled  to  track  the  boundary  in  a  given  direction. 

When  there  are  multiple  static  obstacles  and  a  fixed  target  in  the  en¬ 
vironment,  in  order  to  navigate  the  robot  to  the  target  collision  free,  we 
construct  virtual  obstacle  boundaries  based  on  local  measurements  of  the 
vehicle.  These  virtual  boundaries  are  straight  lines  which  go  through  the 
target.  Algorithm  4.7.7  proposed  one  way  to  construct  the  virtual  bound¬ 
aries.  Theorem  4.7.10  states  that  using  such  an  algorithm  and  following  the 
virtual  boundaries,  the  vehicle  is  able  to  reach  the  target  without  collision. 

In  the  context  of  the  man-made  satellites,  a  space  of  Keplerian  orbits  for 
single  satellite  is  defined  in  [7]  and  [5].  We  point  out  that  if  the  earth  is 
considered  as  a  member  of  this  two-body  formation,  the  space  of  Keplerian 
orbits  can  be  seen  as  part  of  a  shape  space.  When  the  energy  of  each  orbit  is 
the  same,  although  the  geometric  shape  of  the  formation  goes  under  periodic 
changes,  the  difference  in  the  mean  anomalies  between  members  are  constant. 
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Thus  we  name  the  space  of  Keplerian  orbits  of  multiple  satellites  plus  the 
differences  between  the  mean  anomalies  as  the  shape  space  for  a  satellite 
formation.  [47]. 

In  chapter  5,  we  derived  the  Gaussian  perturbation  equations  using  shape 
variables.  We  then  propose  a  Lyapunov  function  based  control  which  is 
able  to  set  up  a  periodic  satellite  formation  on  elliptic  orbits  and  proved 
its  convergence  in  theorem  5.5.2.  In  the  next  step,  we  address  the  issue 
of  setting  up  satellite  formations  on  circular  orbits.  The  difficulty  in  this 
case  lies  in  the  fact  that  on  a  circular  orbit,  the  mean  anomaly  cannot  be 
measured  from  the  perigee  since  there  is  no  perigee  or  apogee.  The  problem 
is  solved  by  measuring  the  mean  anomaly  from  the  ascending  node  instead 
of  the  perigee.  Theorem  5.6.2  provides  convergence  results  for  the  transfer 
algorithm  to  set  up  formation  on  circular  orb  its  [48]. 

1.2  Lie  groups  and  group  actions 

This  section  provides  a  concise  review  of  knowledge  related  to  Lie  groups 
which  is  necessary  in  understanding  the  proofs  and  technical  details  in  the 
following  chapters.  However,  most  of  the  results  in  this  thesis  are  accessible 
without  such  knowledge.  We  also  try  to  explain  the  concepts  in  the  context 
where  it  is  needed. 

An  algebraic  set  G  is  called  a  group  if  the  following  conditions  are  true: 


1.  It  is  equipped  with  a  binary  operation  between  members  of  the  set. 


Furthermore,  the  operation  is  closed  and  associative  i.e.  if  g,h,k  E  G, 
then  g  ■  h  E  G  and  (g  ■  h)  ■  k  —  g  ■  (h  ■  k).  One  often  writes  g  ■  h  as  gh. 

2.  There  exists  a  special  member  id:  called  the  identity  such  that  id  ■  g  = 

9-id  =  g. 

3.  For  any  member  g,  there  exists  a  unique  member,  denoted  by  g such 
that  g  ■  g~l  =  g~l  ■  g  =  id. 

Members  of  a  group  are  often  called  group  elements.  We  say  a  group  is 
Abelian  if  g  ■  h  =  h  ■  g  for  all  elements. 

If  the  notion  of  open  set  is  defined  on  a  group  G  s.t.  (1)  0  and  G  are  open 
sets,  (2)  the  intersection  between  two  open  sets  is  open,  and  (3)  unions  of 
countable  open  sets  are  open,  we  say  G  has  a  topology.  Furthermore,  if  the 
topology  is  well  defined  s.t.  the  group  operation  and  the  inverse  mapping 
that  maps  g  to  g -1  are  continuous,  then  G  becomes  a  topological  group. 

A  Lie  group  is  a  topological  group  where  the  group  operation  and  the 
inverse  mapping  are  differentiable.  Hence,  a  Lie  group  not  only  has  the 
algebraic  properties  of  a  group,  but  also  is  viewed  as  a  manifold  with  metric 
and  geometric  properties. 

The  most  intensively  studied  Lie  groups  are  matrix  groups.  The  group 
elements  are  finite  dimensional  matrices.  The  group  operation  is  the  matrix 
multiplication  and  the  inverse  mapping  is  the  matrix  inverse.  It  is  known 
that  all  finite  dimensional  Lie  groups  can  be  represented  by  matrix  groups. 
Therefore  the  study  of  matrix  groups  is  very  important  both  practically  and 
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theoretically. 

Throughout  the  thesis  we  are  going  to  use  only  two  matrix  groups,  the 
orthogonal  group  O(n)  and  the  special  Euclidean  group  SE(n)  where  n  is  a 
positive  integer.  Furthermore,  in  most  cases  we  let  n  —  2  or  3. 

The  Orthogonal  group  0(n )  is  the  collection  of  all  n  x  n  matrices  g  sat¬ 
isfying  gTg  =  Inxn  where  T  denotes  the  matrix  transpose.  0(n)  has  two 
disconnected  pieces.  One  piece  contains  all  the  g  s.t.  det(g)  =  —1.  The 
other  piece  contains  all  the  g  s.t.  det(g)  =  1.  We  call  the  piece  where 
det(g)  =  1  the  special  orthogonal  group  SO{n). 

The  special  Euclidean  group  SE(n )  is  constructed  by  combining  SO(n ) 
and  the  Euclidean  vector  space  lZn  in  the  following  way.  Let  g  G  SO(n )  and 
r  G  7 Zn,  then  a  group  element  h  G  SE(n )  is  a  (n  +  1)  x  (n+  1)  matrix  of  the 
following  form 


Given  a  Lie  group  G  and  a  manifold  M,  in  many  situations  we  are  able 
to  define  an  operation  <f>fl  for  each  g  G  G  on  every  q  G  M.  If  the  operation 
is  smooth  in  both  g  and  q,  [q]  and  &id[q]  =  q,  we  call  <f>  a 

left  action  of  G  on  M.  For  example,  if  G  is  SO{n )  and  M  is  lZn,  we  can 
define  =  gq.  When  n  =  3,  this  action  produces  a  rotation  of  vector  q. 
If  instead  of  =  <&gh [q]  one  has  $^[$^[5]]  =  <h/)9[?],  then  <f>  is  called  a 

right  action.  We  often  denote  a  left  action  by  Lg  and  a  right  action  by  Rg.  If 
G  is  SE( 3),  then  the  left  action  of  h  G  G  on  VO  is  obtained  by  first  enlarging 
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x  e  7 Z3  to  a  vector  of  four  elements  (xT,  1)T  and  then  multiply  by  the  4x4 
dimensional  matrix  h  to  the  left. 

For  each  q  in  M,  the  action  <f>  produces  an  orbit,  which  is  a  subset  of  M 
defined  as  Oq  =  {x  £  M\x  =  $9[g]  for  some  g  G  G}.  An  action  is  free  if  all 
group  orbits  are  diffeomorphic  to  the  group  G.  Another  way  of  saying  this  is 
that  <Fg[g]  =  &h[q]  if  and  only  if  g  —  h.  For  example,  the  left  action  of  SO (2) 
on  1Z2  —  {0}  is  free,  but  the  left  action  of  SO (3)  on  7 Z3  is  not  free  because 
there  is  a  subgroup  of  rotations  that  fixes  a  given  vector. 

Because  G  is  also  a  manifold,  at  any  point  h0,  one  can  draw  curves  passing 
through  this  point.  The  linear  approximation  of  a  curve  near  ho  is  a  tangent 
vector.  The  collection  of  all  tangent  vectors  is  called  the  tangent  space, 
denoted  as  ThoG.  A  tangent  space  is  well  defined  at  each  point  (  which 
is  also  a  group  element  )  on  this  manifold.  The  dimension  of  the  tangent 
space  is  proved  to  be  the  same  as  the  dimension  of  the  Lie  group. 

The  Lie  group  G  acts  on  itself  through  the  group  operation.  The  action 
can  be  a  left  action  or  a  right  action.  The  left  self  action  maps  curves  near 
a  point,  say  h0  to  curves  near  a  new  point,  say  h\ .  Therefore,  it  induces 
an  operation  on  the  tangent  vectors  by  translating  one  vector  in  the  tangent 
space  of  ho  to  a  new  vector  in  the  tangent  space  of  hi.  We  would  like  to  define 
this  induced  operation  as  an  action.  Because  the  operation  involves  tangent 
spaces  at  every  point  on  the  Lie  group,  it  is  necessary  for  us  to  consider  the 
collection  of  all  tangent  spaces  of  the  Lie  group  G.  We  define  this  large  space 
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as  the  tangent  bundle  TG.  Formally, 


TG=  |J  ThG  (1.2) 

heG 

We  define  the  induced  action  on  the  tangent  spaces  as  the  lifted  action  T$g 
on  the  tangent  bundle  TG. 

Under  the  lifted  action  T<f>3,  starting  from  an  element  £  of  the  tangent 
space  near  the  identity,  the  orbit  is  a  vector  field  on  G  viewed  as  a  manifold. 
This  vector  field,  denoted  by  X%,  is  called  a  left  invariant  vector  field,  because 
(1)  the  vector  at  each  point  can  be  obtained  by  transporting  the  tangent 
vector  £  near  the  identity  using  the  lifted  left  action  and  (2)  if  the  lifted 
action  T<f>9  is  applied  to  this  vector  field,  the  resulting  vector  field  is  the 
same  as  the  original  one. 

For  a  Lie  group  G,  the  tangent  space  at  the  identity  is  called  its  Lie 
algebra,  denoted  by  0.  The  Lie  algebra  is  equipped  with  a  bracket  operation 
which  is  defined  as 

K,I>]|  =  {Xc.XMi.  (1.3) 

where  the  bracket  between  the  two  left  invariant  vector  field  is  the  Jacobi-Lie 
bracket  defined  as 


(A'.  Y]L(f)  =  X(Y(f))  -  Y(X(f))  (1.4) 

where  /  is  a  smooth  function  on  the  Lie  group.  For  G  =  SO(n),  its  Lie  alge¬ 
bra.  is  denoted  as  so(n)  which  contains  n  x  n  skew-symmetric  matrices.  And 
the  Lie  bracket  is  just  the  bracket  operation  between  matrices.  When  n  —  3, 
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the  Lie  algebra  so( 3)  is  diffeomorphic  to  7 Z3  and  the  Lie  bracket  is  the  cross 
product  between  vectors.  Therefore,  it  is  often  more  convenient  to  represent 
elements  of  so(3)  as  three  dimensional  vectors.  In  such  a  context,  one  often 
use  £  for  the  vector  representation  and  use  £  for  the  matrix  representation. 
We  have,  for  £,  77  G  so(3), 

[Zv\i  =  £  x  r)  .  (1.5) 

For  an  element  £  G  g,  we  define  the  exponential  map  exp  :  0  — >  G  s.t. 

^exp(t£)|f=o  =  £  ,  (1-6) 

and  exp ((£  +  s)£)  =  exp(f£)exp(s£).  For  each  £  one  obtains  a  group  element 
exp(£).  The  exponential  map  is  a  cliff eomorphism  between  the  Lie  algebra 
and  an  open  neighborhood  of  the  Lie  group  G  near  the  identity  element. 

An  important  action  is  the  adjoint  action  where  group  G  acts  on  its  Lie 
algebra.  Let  Lg  and  Rg  denote  the  left  and  right  self  action  of  a  Lie  group 
G.  For  £  G  0,  the  adjoint  action  of  G  on  0  is  defined  as: 

M,[£]  =  ^  Lg  [R~ 1  [exp  (££)]]  1 1=0  (1-7) 

For  G  =  50(3),  the  adjoint  action  is 

Ad£]  =  ghf1  or  Adg[t\  =  g£  .  (1.8) 

For  an  element  £  G  G,  there  exists  a  one  parameter  subgroup  exp(t£)  G  G. 
If  we  already  defined  an  action  <F  of  G  on  some  manifold  M,  then  <Fexp(^)  is  the 
action  of  the  one  parameter  subgroup  on  M.  We  can  define  the  infinitesimal 
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generator  of  the  action  as 


^c(g)  = -^$exp(tofe]|t=0  •  (l-9) 

For  G  =  50(3),  the  infinitesimal  generator  is 

&?(?)  =(9  =  (xg.  (1.10) 

Given  a  vector  held  X  ( q )  on  a  manifold  M ,  suppose  G  acts  on  M  by  the 
action  <h9.  A  point  qe  <E  M  is  a  relative  equilibrium  of  X(q)  if  there  exists 
(£0  s.t. 

*(&)=&(&)■  (1-11) 

Unlike  at  an  equilibrium,  the  vector  held  at  a  relative  equilibrium  does  not 
vanish.  Instead,  it  coincides  with  the  tangent  vector  of  a  group  orbit.  We 
note  that  if  the  vector  held  X  is  non-autonomous,  then  £  is  allowed  to  be 
time-dependent.  If  this  is  the  case,  we  say  qe  is  a  relative  equilibrium  for  the 
non-autonomous  vector  held  X ”.(q,t)  if  there  exists  £(£)  G  0  s.t. 

X(qe,t)  =  £G(Qe,t)  ■  (1-12) 
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Chapter  2 


Shape  Space  and  Geometry 


The  many  body  problem  has  been  a  focus  of  classical  mechanics  and  quan¬ 
tum  mechanics  for  more  than  two  centuries.  During  the  last  30  years,  modern 
geometry  started  to  play  an  important  role  toward  getting  insights  on  classi¬ 
cal  results  and  explaining  new  observations.  From  a  geometric  point  of  view, 
the  shape  of  a  formation  is  invariant  under  left  action  of  a  Euclidean  group, 
i.e.  rotation  and  translation.  Under  appropriate  technical  assumptions,  one 
can  split  the  entire  configuration  space  into  two  parts.  One  space  corresponds 
to  a  group  orbit,  which  has  the  same  structure  as  the  group.  The  other  space 
is  the  collection  of  points,  each  point  represents  one  group  orbit.  We  call  the 
first  space  a  fiber  and  the  second  space  the  base  space.  Points  that  belong 
to  the  base  space  describe  the  shape  of  the  formation,  hence  the  base  space 
is  also  called  the  shape  space.  Bearing  the  similarity  with  the  framework  for 
studying  non-holonomic  mechanical  system  (c.f.  [43]),  such  decomposition  is 
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called  a  principal  fiber  bundle. 

Motions  in  the  configuration  space  can  also  split,  resulting  in  motions 
in  the  shape  space  and  motions  in  the  fiber.  One  interesting  phenomenon 
happens  here:  one  motion  in  the  shape  space  may  cause  another  motion  in 
the  fiber.  To  describe  the  relationship  between  the  two  motions,  we  need 
two  mathematical  tools.  First  we  shall  establish  a  gauge  convention,  which 
assign  each  point  on  the  fiber  a  unique  group  element.  We  can  then  slice 
the  configuration  space  into  sections,  each  section  contains  points  that  corre¬ 
spond  to  an  identical  group  element.  Secondly,  we  shall  define  a  connection 
form,  which  describes  the  drift  along  fibers  when  one  tries  to  move  within  a 
section.  In  another  words,  the  connection  form  tells  us  how  much  rotation 
of  a  formation  is  produced  by  changing  the  shape  of  the  formation.  This 
connection  form  is  represented  as  gauge  potentials  in  physics  [23]. 

On  another  track,  mathematicians  working  in  the  field  of  statistical  pat¬ 
tern  recognition  started  to  develop  their  own  notion  of  size-shape  space  from 
the  late  70s  and  early  80s.  Kendall  is  one  of  thcm[15]  [16].  If  we  assume 
all  the  particles  in  a  many  body  problem  have  the  same  mass,  then  a  shape 
space  in  physics  sense  will  be  further  decomposed  into  a  size  variable  and  a 
shape  space  in  Kendall’s  notion.  Hence  the  Kendall  shape  space  is  a  compact 
manifold  equipped  with  a  Riemannian  metric.  This  will  allow  us  to  control 
the  size  and  the  shape  of  a  formation  separately. 

Other  than  the  two  shape  spaces  defined  above,  in  the  context  of  satellite 
formations,  the  space  of  Keplerian  orbits  is  introduced  in  [7]  [5].  Shape  space 
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is  also  defined  when  ensembles  of  rigid  bodies  such  as  G-snakes  [39]  [40]  are 
studied.  Those  shape  spaces  are  utilized  later  in  other  chapters  to  solve 
specific  problems.  In  this  section  we  will  study  the  Jacobi  and  Kendall  shape 
spaces  and  the  geometric  structures  associated  to  them. 


2.1  Jacobi  shape  space  of  the  many-body  prob¬ 
lem 


To  describe  the  motion  of  a  cluster  of  particles,  we  set  up  a  fixed  coordinate 
frame  first.  We  call  it  the  space  frame  or  the  fixed  lab  frame.  Let  qt  G  7 £3, 
%  =  1,2,...,  TV,  denote  the  coordinates  of  N  particles  with  mass  m;.  The 
kinetic  energy  of  this  cluster  is 

1  N 

=  (2.1) 

i= 1 

This  kinetic  energy  is  invariant  under  translation,  because  if  the  positions  of 
all  particles  are  subjected  to  the  transform  qt  — >  q%  +  zc  for  all  i  where  zc  is 
a  given  vector  in  7 Z3,  then  the  value  of  Ktot  is  unchanged.  Let 

N 

M  =  J2mi.  (2.2) 

i=l 
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We  can  see  qc  is  the  coordinate  for  the  center  of  mass.  Now  one  can  define  a 
new  set  of  coordinates  Cf,:  as 


Cfj  Qi  Qc  • 


(2.4) 


Then  the  kinetic  energy  can  be  expressed  as 

N 

'  2 


Ktat  =  \m  ||  qc  ||2  +  i  m,  ||  Cfi 


(2.5) 


i=  1 


Hence  A”  is  separated  into  two  parts.  The  first  part  is  the  translational  kinetic 
energy  which  only  depends  on  ||  qc  ||.  The  second  part  is  the  internal  kinetic 
energy  which  only  depends  on  the  internal  variables  Cfj.  But  since 


N 


5  m.Cf.  -  0, 


(2.6) 


i=  1 


we  can  find  (At— 1)  independent  vectors  (pu,  i  —  1,  2, ...,  At— 1)  from  span(c{i,  i 
1,2,...,  At).  We  want  pa  be  chosen  such  that  the  kinetic  energy  has  the  form 

A-“  =  iM||4ol|2  +  iEl|pK||2  • 

i= 1 

Such  a  set  of  pa  are  called  the  Jacobi  coordinates. 

One  way  of  constructing  the  Jacobi  coordinates  is  to  let 


(2.7) 


Pfl 

Pf2 


V^I(cf2  —  cfl) 


\/p2(Cf3 


mi  Cfi  +  m2cf2 


m  i  +  rri2 


Pii 


V7h(cfp+i) 


— i  (TlfcCffc 

EUl 


18 


U  >  i;  i  <  j) 


(2.n) 


As  one  can  see  from  the  definitions  and  figure  2.1,  the  vectors  pa  are 
constructed  by  finding  the  scaled  relative  displacement  between  the  (i  +  l)th 
particle  and  the  center  of  mass  of  the  sub-cluster  of  first  i  particles.  This 
process  depends  on  how  the  particles  are  labeled.  We  can  also  change  the 
way  we  sub-cluster  particles  as  in  [2],  [22]  and  [23].  Hence  Jacobi  coordinates 
are  not  unique.  However,  between  any  two  sets  of  Jacobi  coordinates  there 
exists  an  element  h  €  0(N  —  1)  s.t. 

[Pfi>  Pf2>  •■■■>  Pf(jv— i)]  =  [Pfi>  Pf2>  ■■■)  Pf(w-i)]h  •  (2.12) 

This  representation  of  the  orthogonal  group  0(N  —  1)  is  called  the  democracy 
group  [23]. 

The  existence  of  the  democracy  group  gives  us  an  immediate  advantage 
in  computing  the  transform  from  (gc,  pn,  Pf2, . . . ,  pf(w-i))  to  (gi,  q-2, . . . ,  g^v) 
i.e.  the  inverse  of  the  transform  that  defines  qc  and  pu .  If  we  can  compute 
the  inverse  map  for  one  set  of  Jacobi  variables,  say  then  for  another  set 
of  Jacobi  variables,  the  inverse  map  $2  1  can  be  obtained  by 

10 

$2  =  $r  •  (2.13) 

0  h 

Therefore  it  is  only  necessary  to  compute  one  such  inverse  map,  namely  JA1. 
We  have  the  following  lemma: 


20 


Lemma  2.1.1  Let 


1 


1 


£}=i 


V^i+l 
2^j  =  l 


mj 


(2.14) 


for  i  =  (1,  2, . . . ,  N  —  1).  Suppose  pu,  (i  =  1,  2, . . . ,  IV  —  1)  are  defined  by 
equation  (2.8).  Then 


qk  =  qc  + 


N—l 

Pf(fc-l)  —  ^  i 

j=k 


(2.15) 


where  we  let  pf0  =  0  and  k  =  1,2, N 

Proof  By  using  equation  (2.8),  we  can  verify  that 


qk+i  —  qk 


N- 1 


Pf(fc-l) 


q\  =  qc  -  ,  (2.16) 

3= 1 

for  k  =  1,  2, . . . ,  N  —  1,  by  evaluating  the  right  hand  side.  We  can  also  verify 
that  qk  given  by  equation  (2.15)  satisfies  equation  (2.16).  But  in  the  non¬ 
degenerate  case  the  solution  to  equation  (2.16)  is  unique.  Therefore  (2.15) 


gives  the  transform  from  (, qc ,  pa,  pn,  ■  ■  ■ ,  Pf(jv-i))  to  (qi,  g2,  •  •  • ,  gv)-  ■ 

Let  Q  =  1Z 3iV  be  the  total  conhguration  space  of  the  formation.  The 

space  of  Jacobi  coordinates  is  1Z3N~3.  We  define 

N- 1 

i= 1 

This  K  is  invariant  under  the  diagonal  left  action  on  1Z3N~3  by  the  special 
orthogonal  group  G  =  50(3).  The  action  is: 


®g(pn)  =  gpu  for  g  G  G  . 


(2.18) 
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This  symmetry  group  G  acts  on  7 Z3N~3  properly  and  freely  except  for  the 
shapes  where  all  pa  are  collinear.  We  let  the  set  F0  be  the  set  of  all  the  Jacobi 
coordinates  corresponding  to  collinear  shapes.  Let  F  =  7 Z3N~3  —  F0  and  call 
it  the  Jacobi  pre-shape  space.  It  is  an  open  sub-manifold  of  the  configuration 
space.  Since  G  acts  properly  and  freely  on  F,  the  base  space  B  =  F/G  is 
a  smooth  manifold  and  the  canonical  projection  n  :  F  — >  B  is  differentiable. 
B  is  called  the  Jacobi  shape  space. 

In  dropping  from  F  to  B,  we  get  rid  of  the  SO( 3)  symmetry  from  the 
Jacobi  coordinates.  After  the  reduction,  the  dimension  of  the  shape  space  B 
is  (37V  —  6).  On  this  shape  space  we  can  define  shape  coordinates  Sj  as 

Sj  =  Sj(pfi,pf2,  /t>f(jv— i))  f°r  j  =  1,2,  ...(37V  -  6)  (2.19) 

s.t. 

sj(gpti,  gpn,  ■■■,  gpf(N-i))  =  Sj(pn,  pk,  •••,  p^n-i))  (2.20) 

for  all  g  G  50(3).  Candidates  for  s-7  are  functions  of  dot  products  (pu  ■  py) 
and  triple  products  (pu  ■  (py  x  pffc)).  Thus,  mutual  distances,  bearing  angles, 
areas  and  volumes  formed  by  the  line  segments  connecting  the  particles  all 
serve  as  candidates  for  shape  variables. 

One  can  establish  a  body  coordinate  system  on  a  formation  with  certain 
shape.  The  reference  orientation  of  this  formation  can  be  defined  as  the 
orientation  when  the  body  coordinate  frame  and  the  lab  coordinate  frame 
coincide.  Then  the  orientation  of  this  formation  with  the  same  shape  can 
be  described  by  an  element  g  G  50(3).  The  Jacobi  coordinates  in  these  two 
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coordinate  systems  have  the  following  relationship: 


Pfi  QPiis) 


( 2.21 ) 


where 

s  =  (si,  S2, s3Ar_6)T.  (2.22) 

Pi  are  Jacobi  coordinates  in  the  body  coordinate  frame  which  only  depend 
on  shape  coordinates. 

Taking  derivative  on  both  sides  of  (2.21),  we  get 


Pa  9Pi  +  9Pi  • 


(2.23) 


On  a  matrix  Lie  group  G,  g  G  Tg G.  There  exists  fi  e  g  the  Lie  algebra  s.t. 
g  =  g£l  .  In  our  case,  G  =  50(3),  so  g  =  (7 Z3,  x ).  Thus  the  derivative  of  pa 


is 

3  V— 6  rs 

Pa  =  g(Qx  Pi+  ^2  - .  (2.24) 

3= 1  J 

In  the  body  coordinate  frame,  the  angular  momentum  of  the  whole  system 
J  can  be  calculated  as 


v-i 

J  =  9  *  Pf») 

1=1 
3  TV-6 

=  (2-25) 

i=  1 


where 

v-i 

7(«)  =  Pill2  e-PiPD  (2.26) 

1=1 
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is  defined  as  the  locked  inertia  tensor  of  the  formation  in  the  body  coordinate 


frame  and 


TV— 1 


Aj(S)  =  1  1  ^  Pi 


X 


1=1 


dpi 

dSj 


(2.27) 


are  vector  potential  functions.  These  quantities  are  defined  on  the  shape 
space  because  pt  only  depend  on  shape  coordinates. 


2.2  Gauge  transform 

As  discussed  in  the  previous  section,  for  a  given  shape  s0,  one  can  establish  a 
body  coordinate  system  and  obtain  a  group  element  in  5*0 (3)  which  measures 
the  orientation  of  the  formation  and  serves  as  the  transform  between  the  lab 
coordinate  system  and  the  body  coordinate  system.  If  the  shape  of  the 
formation  has  changed  to  Si,  in  order  to  compare  the  orientation,  one  has 
to  make  sure  that  the  body  coordinate  system  is  established  in  a  consistent 
way.  Therefore,  the  procedure  to  establish  a  body  coordinate  system  should 
be  shape  independent.  Such  a  shape  independent  procedure  for  establishing 
body  coordinate  system  on  a  deformable  body  is  called  a  gauge  convention. 
Formally,  a  gauge  convention  is  a  diffeomorphism  between  F  and  G  x  B 
mapping  any  point  pf  G  F  to  (g,  s)  e  G  x  B  s.t.  $91[pf]  is  mapped  to 
(gig,s).  Notice  that  g  serves  as  part  of  the  coordinates  for  pf  independent 
of  s.  For  example,  let  ( Qi,q2,Q3 )  describe  three  points  in  1Z3.  Except  for 
the  singular  cases  of  collinear  shapes,  one  can  establish  the  body  coordinate 
system  of  this  triangular  formation  as  follows: 
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1.  Place  the  origin  at  center  of  mass,  let  the  re- axis  be  aligned  with  vector 
?1?2- 

2.  Let  the  y-axis  be  in  the  plane  formed  by  triangle  perpendicular  to 

the  x-axis  and  points  towards  point  q3. 

3.  Let  the  z-axis  form  a  right  handed  coordinate  system  with  x-axis  and 
y-axis. 

It  is  easy  to  see  that  this  method  is  truly  shape  independent. 

Gauge  conventions  are  generally  not  unique.  Let  g  G  50(3)  describes  the 
orientation  of  the  formation  for  any  s  G  B  under  one  gauge  convention.  Let 
gi  G  50(3)  describes  the  orientation  of  the  formation  for  the  same  s  G  B 
under  another  gauge  convention.  Then  by  the  property  of  50(3),  there  exists 
h(s)  such  that 

g  =  gihT(s)  (2.28) 

where  h  :  B  — »  50(3)  is  a  50(3)  valued  function  on  B.  This  right  action  of 
h(s)  on  50(3)  is  called  a  gauge  transform.  Because  a  gauge  transform  is  a 
shape  dependent  group  action,  an  object  which  obeys  certain  transformation 
rules  under  rigid  group  action  by  50(3)  may  violate  such  rules  under  a 
gauge  transform.  We  say  an  object  is  gauge  invariant  if  it  is  invariant  under 
any  gauge  transform.  We  say  an  object  is  gauge  covariant  if  it  obeys  the 
transformation  rules  for  rigid  group  action  by  50(3)  when  it  is  subjected 
to  gauge  transforms.  An  obvious  example  of  a  gauge  invariant  object  is  the 
shape  variable  s.  The  tangent  vectors  to  the  shape  space  B  are  also  gauge 
invariant.  An  obvious  example  of  a  gauge  covariant  object  is  the  Jacobi 
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vectors  pi.  Because 


gpi  =  Pu  =  gipn  ,  (2.29) 

this  implies  that 

Pi  =  h(s)pn  .  (2.30) 

Hence  pt\  is  transformed  to  p*  via  the  left  action  by  50(3). 

In  the  applications  of  cooperative  control,  gauge  invariance  and  gauge 
covariance  are  desired  because  of  the  distributed  nature  of  the  computing 
power.  Each  agent  within  a  team  has  its  own  freedom  or  limitation  on  how 
to  describe  the  combined  system.  A  gauge  invariant  or  covariant  object  can 
be  shared  across  multiple  agents  with  little  or  no  conversion. 

Let  F  denote  the  Jacobi  Pre-shape  space  coordinated  by  (g,  s )  where 
g  G  50(3)  and  s  =  (s^,  s2,  ■  ■  ■ ,  s3n-q)  £  B  where  B  is  the  Jacobi  shape 
manifold.  A  point  in  the  tangent  space  TF(g,s)  can  be  represented  by 
(g,s,gQ,v)  where  Q  e  so( 3).  However,  such  representation  of  TF  is  not 
gauge  covariant.  Linder  a  gauge  transform 

g  =  gihT(s)  ,  (2.31) 

then  we  can  calculate  the  transform  of  as 

n  =  g~lg 

=  h(s)gl(g1hT(s)FglhT(s)) 

=  h(s)VlihT  (s)  +  h(s)hT  (s) 

=  h(s)(Qi  +  hT (s)h(s))hT (s)  .  (2.32) 
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Since  hT(s)h(s )  =  id  where  id  is  the  identity,  we  know  that  hT(s)h(s)  = 


—hT(s)h(s).  We  then  define 


T  dh 

Ti  =  hT  —  for  i  —  1,2, ,  3N  —  6 

OSj 


(2.33) 


It  is  easy  to  see  that  Ik  e  so(3).  Therefore,  we  can  let  y*  denote  the  vector 
representation  of  T, .  Hence  equation  (2.32)  becomes 


3N—6 


H  =  h(s)(Q i  —  TjSi)h 1 


(2.34) 


i=  1 


Its  vector  form  is 

H  =  h(s)(£l i  -  ^  7iS*)  .  (2.35) 

It  is  clearly  not  gauge-covariant  because  Hi  can  not  be  transformed  to  H  by 
any  lifted  action  of  50(3)  on  so(3).  We  now  define 


3JV-6 


3N—6 

t  =  n  +  A 

3= i 

Lemma  2.2.1  T  is  gauge  covariant  and 


3S 3  ■ 


(2.36) 


T  =  h(s)  Tx 


(2.37) 


under  the  gauge  transform 


g  =  gihT(s ) 


(2.38) 


Proof 

Under  the  gauge  transform,  a  Jacobi  vector  is  transformed  as 


Pi  =  h(s)pn  . 


(2.39) 
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Therefore,  according  to  the  definition  of  the  locked  inertia  tensor  in  equation 
(2.26),  one  can  verify  that 


I(s)  =  h{s)h(s)hT(s )  (2.40) 


i.e.  the  locked  inertia  tensor  is  gauge  covariant. 

According  to  equation  (2.27), 

A,  =  r'"fWsWx^l 

i=  1  -1 

=  h(s)I{1hT(s)h(s)  Y  Pa  x  +  hT(s)^^-pa) 

i=  1  ^  ^ 

V-l 

=  Ms)(4*i  +  Ix  1  ^  pa  x  (7 j  x  pa)) 

i=  1 
N- 1 

=  Ms)  (4*i  +  A'1  X](ll  Pti  II2  -  piipDij) 

—  h(s)(Aji  +  7j)  .  (2-41) 


Therefore, 

3V— 6 

T  =  4'Sj 

j=l 

3V-6  3V-6 

=  /l(5)(fi!  -  E  7jSj)+  ^  ^(s)(4i  +  7t)«j 

j=i  i=i 

3V-6 

=  /l(s)(^i  +  E  4*i®t) 

t=i 

=  /i(s)Ti  .  (2.42) 


Thus,  a  gauge  co-variant  coordinates  for  a  point  in  the  tangent  space 


TF(g,  s)  is  (g,s,g T,v). 


2.2.1  Connections  and  gauge  kinematics 

In  the  previous  sections,  objects  such  as  /,  J  and  A  arise  when  we  tried 
to  write  the  kinetic  energy  in  a  split  form.  We  have  given  formulas  for 
computing  those  objects  in  either  the  fixed  lab  coordinate  system  or  the 
body  coordinate  system.  Although  the  results  are  necessary  and  preferred 
by  engineering  applications,  the  mathematical  properties  of  these  objects  are 
not  fully  shown.  Therefore,  we  would  like  to  redefine  the  objects  including 
/,  J  and  A,  and  concepts  such  as  gauge  convention  and  gauge  transform,  by 
taking  a  geometrically  intrinsic  approach.  This  approach  provides  a  clearer 
image  for  the  underlying  structure  of  the  problem  and  may  also  lead  to 
generalization  of  our  results  to  a  larger  class  of  systems. 

A  function  with  multiple  arguments  can  often  be  viewed  as  an  operator 
acting  on  the  domain  of  one  of  its  arguments.  The  other  arguments  can  then 
be  viewed  as  parameters  for  this  operator.  To  distinguish  the  argument  being 
operated  from  the  parameters,  we  use  [  ]  to  enclose  the  argument  and  use  (  ) 
to  enclose  the  parameters. 

We  first  point  out  that  (F,  B ,  n,  G)  forms  a  principal  fiber  bundle  with 
shape  space  at  the  base  and  SO( 3)  orbits  as  fibers.  Let  TPtF  be  the  tangent 
space  of  F  at  point  pf  G  F.  Let  .s  =  vr(pf)  be  the  base  point  of  pf  in  B.  The 
tangent  space  to  B  at  point  s  is  TSB.  Let  TF  =  UPfeF  B^F,  TB  =  lJse_B  TSB. 
TF,  TB  are  called  the  tangent  bundles  of  F  and  B  respectively.  Here,  a  point 
of  the  tangent  bundle  TF  represents  a  point  pf  of  F  paired  with  a  tangent 
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vector  vPf  to  F,  while  a  point  of  the  tangent  bundle  TB  represents  a  point 
soiB  paired  with  a  tangent  vector  vs  to  B.  Since  the  canonical  mapping  n 
maps  pi  to  s,  if  the  tangent  vectors  to  F  and  B  are  viewed  as  infinitesimal 
approximations  for  curves  in  F  and  B  passing  through  pf  and  s,  then  n 
induces  a  tangent  map  Ttt  which  maps  a  tangent  vector  vPi  in  Tpf  F  to  a 
tangent  vector  vs  in  TSB.  Therefore,  the  map  ( )  defines  a  projection 
map  from  tangent  bundle  TF  to  tangent  bundle  TB.  This  projection  map 
(7 r,  Ttt)  annihilates  tangent  vectors  to  the  fibers  (group  orbits).  Such  tangent 
vectors  to  the  fibers  are  closely  related  to  the  tangent  vectors  to  the  Lie  group 
G. 

For  a  Lie  group  G,  the  tangent  space  at  the  identity  is  called  the  Lie 
algebra  associated  with  G,  denoted  by  g.  Then  any  curve  g(t)  G  G  passing 
g(0)  when  t  —  0,  can  be  expressed  as  \h9(0)[exp(t^)]  where  T  denotes  the 
group  operator,  £  denotes  a  vector  in  g  and  exp(-)  is  the  exponential  map 
between  tangent  space  and  base  space.  If  we  take  derivative  with  respect  to 
t  along  those  curves,  we  obtain  a  linear  map  Tg^  T  :  g  — >  Ts(0)  G  between  the 
Lie  algebra  and  the  tangent  space  at  g( 0).  Furthermore,  any  tangent  vector 
to  the  Lie  group  G  at  point  g(0)  can  be  represented  by  TS(0)'L[£]. 

Other  than  TS(0)'F,  we  can  define  another  linear  operator  Adg (0)  :  g  — >  g, 
which  we  call  the  adjoint  operator ,  as  follows: 

Ada( o)[f]  =  ^^3(o)[^exp(to[5,_1(0)]]  »  (2-43) 

where  £  G  g. 
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We  now  compute  the  tangent  vector  to  the  fibers  of  F.  Let  £f{P{)  £  TPiF 
be  the  infinitesimal  generator  of  the  group  action  <f>9  on  F  induced  by  £  €  g, 
which  is  defined  as 

^(Pf)  = -^d>exp(t$)[pf]|t=o  .  (2.44) 

Then  the  tangent  vector  to  a  fiber  of  F  at  point  pf  =  (g,  s )  is 

^<b'I'9[exp(^)][pf]|<=0  =  ^(®g^exp(t£)[pf])|t=0 

=  T$g[^(Pf)]-  (2.45) 

This  equation  implies  that  at  any  given  point  in  F,  there  is  a  one  to  one 
correspondence  between  a  tangent  vector  to  the  fiber  and  a  vector  in  the  Lie 
algebra  0  of  the  Lie  group  G. 

The  map  TV  and  the  infinitesimal  generator  indicate  that  the  tangent 
bundle  TF  can  be  viewed  as  a  combination  of  the  tangent  bundle  TB  and 
a  Lie  algebra  0.  Therefore,  a  tangent  vector  to  F  can  be  represented  by  a 
tangent  vector  to  B  and  a  vector  in  the  Lie  algebra.  However,  this  separation 
does  not  mean  that  TB  and  0  are  decoupled  or  unrelated.  In  fact,  because  of 
the  existence  of  a  Riemannian  metric  on  F,  there  exists  interesting  coupling 
between  TB  and  0.  Mathematically,  we  say  there  exists  a  connection  between 
a  tangent  vector  to  F  and  a  vector  in  0. 

To  describe  such  a  connection,  we  need  a  mathematical  object  called  a  lie 
algebra  valued  one  form.  It  is  a  linear  operator  u>{  :  TF  — >  0  represented  as 
u;(pf)[vf]  for  pf  G  F  and  Vf  e  TPfF.  Concerning  the  group  action  and  the 
induced  tangent  operator  T$g,  we  use  the  notation  $*o;f(pf)[uf]  to  represent 


31 


o;($9[pf])[T$s[vf]].  is  called  the  pull-back  of  the  one  form  u>{.  We  call 

u>f  a  connection  form  on  F  if  the  following  two  properties  are  satisfied: 

(1)  cnf(pf)[£F]  =  £ 

(2)  &gUH{pi)[X\  =  Adg[cv{[X ]] 

for  all  vector  fields  X  E  TF.  This  connection  form  tells  us  what  is  the  vector 
in  0  that  is  associated  with  a  tangent  vector  to  F.  It  also  puts  a  restriction 
on  this  association  by  requiring  that  the  vector  in  g  must  satisfy  the  second 
property  under  group  action  <3>r  Specially,  if  v  —  T<bg[fF\  is  a  tangent  vector 
to  a  fiber  at  certain  point  pf  =  (g,  s).  Then 

ut[v]  =  Wf[T$fl[£F]]  =  **guf[tF]  =  Adg[f]  .  (2.46) 

Once  a  connection  form  ujf  is  defined,  a  given  vector  field  X  with  X{pf)  E 
TPiF  can  be  decomposed  as 

X(pt)=Xv(pi)  +  Xh(pi)  (2.47) 

with 


^[Xv{Pi)}  =  Wf[X(pt)\ 

ut[Xh(pt)]  =  0.  (2.48) 

Here,  Xv  and  Xh  are  called  vertical  and  horizontal  components  of  X.  This 
suggests  that  the  tangent  space  TptF  can  be  decomposed  into  vertical  sub¬ 
space  VPf  and  horizontal  subspace  HPf. 

TPfF  =  VP{®HPi  (2.49) 
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where 


VP{  =  {ui  e  TptF \vi  =  T$g[£F],  for  some  £  G  g} 

HP{  =  {v2  G  TPfF \u{[v2\  =  0}  .  (2.50) 

Since  the  vertical  vectors  are  tangent  vectors  to  fibers,  we  know  that  TPfn[v]  = 
0  for  v  G  Vpr  This  is  to  say  that  the  projection  of  TptF  on  to  TptB  eliminates 
the  vertical  space  VP{. 

On  a  principal  fiber  bundle,  usually  the  connection  form  uq  is  not  uniquely 
defined.  For  a  certain  class  of  system  called  the  simple  mechanical  system 
with  symmetry,  a  canonical  connection  form  called  the  mechanical  connection 
exists. 

A  simple  mechanical  system  with  symmetry  is  a  quintuple  (Q,  Km ,  G,  V,  <L). 
Q  is  the  configuration  space.  Km  is  a  Riemannian  metric  on  Q.  V  is  a  smooth 
function  on  Q.  G  is  a  Lie  group  and  <f>  defines  the  action  of  G  on  Q.  For 
q  G  Q,  \ Km(q,q )  is  called  the  kinetic  energy  and  V(q)  is  called  the  poten¬ 
tial  energy.  The  kinetic  energy  | Km(q,q )  and  potential  energy  V  should  be 
invariant  under  the  group  action  <f>,  that  is, 

Km(*g[Q])(T*g[q],T$g[q])  =  Km(q)(q,q) 

V($g[q\)  =  v(q).  (2.51) 

By  this  definition,  our  formation  system  with  V  =  0  is  a  simple  mechan¬ 
ical  system  with  configuration  space  Q  =  VJ'  x  F ,  kinetic  energy  Ktot  and 
the  group  G  acting  trivially  on  ^(coordinate  of  center  of  mass)  but  diag¬ 
onally  on  F  (the  Jacobi  coordinates).  For  such  a  system,  we  can  define  a 
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locked  inertia  tensor  and  a  momentum  map.  These  two  objects  can  both  be 
viewed  as  linear  operators  on  0.  Thus  they  belong  to  the  dual  space  of  the 
Lie  algebra  0,  denoted  by  0*. 

The  locked  inertia  tensor  of  the  system  is  Jf  :  F  x  0  — >  0*  satisfying 

=  Km(pf)(^F,VF)  ,  (2-52) 

where  £,  rj  G  0  and  pf  G  F. 

The  momentum  map  is  Jf  :  TF  — >  0*  satisfying 

Jf(PfWpf)[£]  =  ^m(pf)(u«,^F)  ,  (2.53) 

where  £  G  0,  pf  G  F  and  uPf  G  TpfF. 

For  the  formation  system,  the  mechanical  connection  is  defined  as  cuf(pf) 
s.t. 

wf(Pf)[u«]  =  /f(Pf)_1['/f(Pf^Vf)]  •  (2-54) 

It  satisfies  the  first  condition  for  a  connection  form  because 

wf(Pf)[^F]  =  ^f(Pf)_1[^f(Pf,^F)] 

=  /f(Pf)_1[/f(Pf,£)] 

=  £  .  (2.55) 

Let  us  now  check  for  the  second  condition  for  a  connection  form.  We  want 
to  know  how  Jf,  Jf  and  Uf  are  transformed  under  the  group  action  <3>p.  We 
first  compute  the  transformation  rule  for  an  infinitesimal  generator  £p(pf). 
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T$g-i[{Adg-i[£ ])F]  . 


(2.56) 


Then, 


KJ-TflPf.OM 


Km(pi)((Adg-4Q)F,(Ads-i[ri])F) 

Ada-4i])lAda-'{n]} 

(Pf.^-iKDDM;,  (2.57) 


and 


=  A'm(4„W)(r4,[«],?F(4s[pt])) 

=  Km  (4 ,  W )  (T<$,  [«] ,  T4,  {(Adr,  [£] )  F] ) 

=  Km(pt)(v,  (Ad,-i[£])F) 

=  (A<-.[Jf(pt,  «)])[«]•  (2.58) 

Here,  Ad T  is  the  conjugate  operator  of  Ad3.  If  Ads  is  represented  by  a  matrix, 
then  Add  is  represented  by  the  transpose  of  the  matrix.  We  can  rewrite  the 
above  results  as 


«>;/,  =  (AdT->[it])[Adg->] 


Q'gJi  =  A<%-l[Jf}- 


(2.59) 


Therefore 

=  Ad^lAd^Ad^lJ,]]] 

=  Ad3[I^[Jt\} 

=  Ad„[ o)f]  ,  (2.60) 

that  is,  uj{  satisfies  the  second  property  of  a  connection. 

With  the  connection  form  well  defined,  we  are  now  ready  to  define  the 
relationship  between  TB  and  0.  We  first  notice  that  the  base  space  B  is 
embedded  in  F  by  a  mapping  r  such  that  tt  o  t  =  him  where  idm  denotes  the 
identity  map.  Unlike  the  mapping  tt,  t  is  not  uniquely  determined.  Let  T\  be 
one  possible  choice  and  T2  be  another.  T\  (s)  and  72(5)  lie  on  the  same  fiber 
(group  orbit)  because  they  are  mapped  to  the  same  point  on  the  base  space 
by  tt.  Then  for  each  s  G  B,  there  exists  a  group  element  h(s)  G  G  s.t. 

ri(s)  =  $/*(*)  [t2(s)]  (2.61) 

We  define  a  gauge  convention  to  be  a  choice  of  map  r.  The  set  t(B)  is  called 
the  zero  section  of  the  principal  fiber  bundle.  A  point  in  t(B)  is  represented 
by  (id,  s )  where  id  is  the  identity  element  of  the  Lie  group  G.  We  define  the 
action  of  the  group  valued  function  h(s)  described  in  equation  (2.61)  as  a 
gauge  transform.  One  can  verify  that  if  the  group  G  is  SO( 3)  and  the  action 
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is  the  left  action,  then  these  definitions  are  equivalent  to  the  definitions 
for  gauge  convention  and  gauge  transform  in  section  2.2. 

Furthermore,  the  map  r(-)  induces  a  tangent  map  Tt  which  maps  a  tan¬ 
gent  vector  to  B  to  a  tangent  vector  to  F.  Then,  we  can  define  the  pull-back 
of  the  connection  form  u  under  the  map  r,  denoted  by  t* u  as 

T*u>[vs\  =  ui[Tt[vs]\  (2.62) 

for  vs  G  TSB.  This  t*co  is  a  one  form  on  B.  It  is  also  called  the  pull-down 
connection  form.  Such  a  form  defines  a  relationship  between  a  tangent  vector 
to  B  and  a  vector  in  the  Lie  algebra  0. 

We  point  out  that  the  notion  of  gauge  invariant  and  gauge  covariant 
can  be  well  understood  in  this  framework.  The  locked  inertia  tensor  if,  the 
angular  momentum  Jf  and  the  connection  form  oq  are  gauge  invariant  since 
they  are  defined  regardless  of  the  choice  for  r.  Next,  we  create  representations 
for  if,  Jf  and  U{  when  they  are  restricted  to  the  zero  section  t(B).  We  define 

I{p,€)  =  -ff(Pf,£)Uer(B) 

J(p,vp)  =  Jf{Pfivf>i)\(pteT(B),vPfeTP{F) 

OJ  =  rl[J]  .  (2.63) 

When  the  group  G  is  SO( 3),  /  and  J  represent  the  locked  inertia  tensor  and 
angular  momentum  in  the  body  coordinate  system.  These  representations 
are  gauge  dependent  because  they  depend  on  the  construction  of  r.  To  see 
what  the  dependency  is,  we  introduce  a  gauge  transform  $h(s),  which  by 
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definition,  produces  p]  =  §h{s) [p2  ]  where  p\  G  ri(£>)  and  p2  G  T2  (.£>).  The 
following  calculations  show  how  I,J  and  u  are  transformed. 


=  If{phZ)UenW 

=  I{{$h(s)[Pf},0\p2£T2(B) 

=  $ft(s)/f(pf,0 

=  Adh-i^iipb  Adh-Hs)it})} 

=  Ad^-i(s)[I(p2^Adh-Hs)[Q)} 

J{pXy^)  = 

=  M®h(s)[p{},T$h(s)[v2pf])\p2eT2{B) 

=  K(s)Mplv2pt) 

=  Adh-i(s)[Mplv2Pf)} 

=  Adh-i(s)[J(p2iv2p )] 

^(p1)  =  iyrljy) 

=  Adh(s)[I  (p2)'1  [AdTh(s)  [A<%-Ha)  [J  (p2)]]]] 

=  Adh{s)[I{p2)-1[J{p 2)]] 

=  Adh{s)[u(p2)\  .  (2.64) 

In  these  transformation  rules,  h(s)  appears  as  if  s  is  a  constant  i.e.  there  is 
no  term  involving  We  say  I,  J  and  uj  are  gauge  covariant  because  the 

transformation  rules  are  the  same  as  the  rigid  body  transformation  rules. 

The  angular  velocity  and  the  pull-down  connection  form  also  depend  on 
specific  gauge  convention.  Because  their  transformation  rules  involve 
they  are  neither  gauge  invariant  nor  gauge  covariant. 
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We  mention  that  although  the  intrinsic  approach  is  abstract,  it  is  able  to 
produce  the  same  set  of  formulas  as  seen  previously  in  both  the  lab  coordinate 
system  and  the  body  coordinate  system.  We  would  like  to  show  this  process 
as  a  validation  to  the  intrinsic  approach. 

In  our  context,  since  the  group  G  is  SO(3),  the  Lie  algebra  0  is  so(3).  We 
can  represent  the  elements  of  so(3)  by  3-dimensional  vectors  £,  r)  and  also  by 
(3x3)  skew  symmetric  matrices  £,  rj. 

In  the  lab  coordinate  system, 

£f(pi)  =  (£  x  Pfi>£  x  Pf2,  •••£  x  Pf(v-i))  •  (2.65) 

By  the  definition  of  the  locked  inertia  tensor,  we  have 

[v]  =  K(£F,Vf) 

N-l 

=  *  ph)t (n  *  pa) 

i— 1 
N-l 

=  ^(H  Pfi  II2  e  “  PfiPti)7!  (2-66) 

i= 1 

where  e  is  the  3x3  identity  matrix.  Hence 

N-l 

Jf(Pf)  =  5Zdl  Pii  II2  e  ~  PUP «)  '  (2-67) 

i=  1 

By  similar  calculations  we  find  the  expression  for  the  momentum  map  J  to 
be: 

N-l 

Jf^Ph  Pi)  ^  ^  Pfi  X  pfi  .  (2.68) 

i= 1 

The  expressions  in  body  coordinates  can  be  derived  by  applying  equation 
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(2.63).  The  locked  inertia  tensor  in  body  frame  is 

JV-1 

I{p)  =  5^(11  All2  e-pipl)  •  (2-69) 

i— 1 

We  can  see  that  If  =  gIgT .  Next,  we  notice  that  if  pf  G  t(B),  then  according 
to  equation  (2.23),  its  coordinates  satisfy 

pa  =  x  pi  +  pi  .  (2.70) 


For  the  momentum  map,  we  have 

N- 1 

J(PiP)  =  ^f(Pf)  Pf)|(pf6r(B),pfeTpfF)  =  ^(Pi  X  (f]  X  p*)  +  Pi  X  p*)  .  (2.71) 

Obviously,  Jf  =  gj.  After  simplification  we  have: 

JV-l 

J (p,  p)  =  /O  +  pi  x  ^  .  (2.72) 

i=  1 

Hence,  the  mechanical  connection  in  body  frame  is 

u{p)\p]  =  j~lj 

N- 1 

=  n  +  r^ftXft.  (2.73) 

i=l 

Therefore,  one  can  see  the  relation  between  ay  and  a;  as, 

^f(Pf)[df]  =  /f"1^  =  =  gw(p)\p]  ■  (2-74) 

After  we  choose  shape  coordinates  Sj  on  the  shape  space  B ,  we  already 
know  that 

Pi  =  Pi(si,  s2,  -s3N-g)  .  (2.75) 
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Therefore,  let  vs  e  TSB,  the  lift  of  vs  to  Tr(s)F  is 


Tst\vs ]  =  —  {pi(s  +  tvs),p2(s  +  tvs),  ...,pN-i(s  +  tvs),id)\t=0 

3N-6  0  3N-6  „  3N—6  „ 


iji  V  — u  ' — u  r\  V  — u  r\ 

(  V"  dp2  ^V-l  n  _  n\  >7c\ 

^E  f)QVs^Z^  L  -  0).  (2.76) 


E  ^  ^  rAs, 

j=i  J  j=i  J 


j=i  j 


Then  by  equation  (2.73), 


T*w(s)[ua]  =  uj(p{)[Tr[vs]]\PteT{B) 

N-l  3N-6  rr) 

=  i~\n+j2(pix  E  ~£~Vs^ 

i=i  j= i  ^ 

N-l  3 N-6  r, 

=  E 

i=l  j= 1  3 

SN—6  N-l  r, 

=  E^E*  *<£>«■ 

j= 1  Z=1  ^ 


Define  A«  as 


Ai  =  r'lY. 


(2.77) 


(2.78) 


Then  the  representation  of  r*cn  in  shape  coordinates  is  E/=i_6  Ajdsj  where 
are  lie  algebra  valued  functions.  Aj  are  called  the  gauge  potentials  on  the 
shape  space  B.  Because 


'"E 


PiX  Pi  = 


E  AjS]  ) 


(2.79) 


we  also  obtained  a  simpler  form  for  u  as 


;(g,s,fl,s)  =  Q+  ^  Aj 


(2.80) 


This  is  identical  with  the  definition  of  T  in  equation  (2.36).  The  gauge 
covariant  property  of  T  comes  from  the  fact  that  to  is  gauge  covariant. 
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We  found  the  information  in  [43]  and  the  references  therein  very  helpful 
in  understanding  gauge  theory.  The  block  diagonalization  technique  can  be 
found  in  [25,  35,  36,  20,  42], 

2.3  Kendall’s  notion  of  size  and  shape 

For  a  while,  statisticians  have  been  asking  the  following  questions:  Given 
data  points  on  a  line,  plane,  ...  7 Zm,  how  can  one  describe  the  shape  of  the 
data?  distinguish  between  two  shapes?  determine  the  distance  between  two 
shapes?  Starting  from  the  knowledge  that  shape  information  is  invariant 
under  translation  and  rotation,  D.G.  Kendall  rediscovered  Jacobi  vectors 
but  with  the  assumption  that  all  the  particles  have  unit  mass  [15].  Given  N 
points  in  Rm,  N  >  2,  as  x\,X2,...,x*N,  written  as  columns  of  matrix  X*.  Then 
Kendall’s  coordinates  are 

xo  =  VNx*c 

=  -j={x\+x*2-\ - h4) 

x.j  =  J_ jjx*+l  0*  +  x*  +  •  •  •  +  x*j))  (2.81) 

VJ+J 

where  1  <  j  <  (N  —  1).  In  matrix  form, 

X  =  [x0,xi,  ...jXjv-r]  =  X*Qn  ,  (2.82) 
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for  example: 


Qd,  — 


1 

-1 

-1 

V3 

V2 

vT 

1 

1 

-l 

V2 

vT 

1 

0 

2 

d3 

vT 

(2.83) 


In  fact,  if  we  let  rn i  =  m2  =,  —  1  in  the  definition  of  Jacobi  coordi¬ 

nates,  then 

*  -  =  jij  ■  (2  84) 

2^k=  1  mk  J  +  1 
Hence  by  letting  qi  =  x*  for  i  =  1,  2, N,  we  have 


n  _  E»=i 
9c  M 

Pij  = 

I  3  /_ 


i  +  iw 

r~r~ 

ju + 1) 


v/a 

ELl  mkcik  \ 
EUmk 
Qi  +  92  +  •••  +  Qj  - 
J 


fe+i - - - Z) 

-  (?l  +  ?2  +  -  +  &)) 


=  X3  ■ 


(2.85) 


Thus  we  have  proved  that  Kendall  coordinates  are  Jacobi  coordinates  with 
the  unit  mass  assumption. 

The  Kendall  size  variable  is  defined  as 

TV  TV— 1 

S  =  \  S  ll  Xi  ~  X*c  II2  =  \  II  ^  II2  •  (2-86) 

\  i  1  \  i= 1 

Assuming  S  ^  0,  we  have  removed  degeneracy  of  total  collision,  correspond¬ 
ing  to  the  case  when  the  formation  will  shrink  to  one  point  so  that  no  shape 
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need  to  be  studied.  After  dropping  the  first  column  in  matrix  A",  then  divide 
the  result  by  S,  we  get  an  (m  x  ( N  —  1))  matrix 

X  =  [x1,x2...,xN-i]  (2.87) 

where  xt  =  Xi/S.  X  is  a  point  in  a  ( m(N  —  1)  —  1)  dimensional  sphere 
which  is  defined  as  the  Kendall  pre-shape  sphere  S^.  A  Kendall  shape  is  an 
equivalence  class  [A]  on  the  pre-shape  sphere  where  the  equivalence  relation 
~  is  : 

X  ~  Y  , 

if  there  exists  T  e  SO(m )  s.t.  X  =  TY  .  The  Kendall  shape  space  is  the 
space  of  such  equivalence  classes,  denoted  by  .  It  is  the  shape  space  of  N 
data  points  in  TZni.  Notice  that  only  the  case  where  all  particles  collide  has 
been  excluded  from  this  shape  space. 

The  similarity  between  Kendall’s  procedure  and  Jacobi’s  procedure  is 
very  clear.  After  the  unit  mass  assumption  is  dropped,  we  can  make  a  small 
extension  to  the  definitions  of  Kendall’s  size  variable  so  that 

N-l 

S=\  ^  II  Pull2-  (2-88) 

N  i=1 

In  fact,  this  size  variable  S  can  be  viewed  as  a  special  choice  of  Jacobi  shape 
variable.  We  extend  the  Kendall  coordinates  to  be  the  Jacobi  coordinates. 
Then  this  extended  Kendall  shape  space  is  a  compact  sub-manifold  of  Jacobi 
shape  space.  All  the  future  arguments  about  Kendall  shape  space  is  applica¬ 
ble  to  the  extended  one  if  we  replace  the  Kendall  coordinates  by  the  Jacobi 
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coordinates. 


From  now  on,  the  terms  shape  variable(s)  or  shape  coordinate(s)  will  refer 
to  variables  or  coordinates  on  the  Jacobi  shape  space.  The  terms  pure  shape 
variable(s)  or  pure  shape  coordinate (s)  will  refer  to  variables  or  coordinates  on 
the  extended  Kendall  shape  space  where  size  information  has  been  removed. 

We  discovered  an  interesting  fact.  The  size  variable  S  is  equivalent  to 
the  virial  function  which  has  been  used  for  measuring  the  size  of  star  clusters 
much  earlier  than  Kendall’s  work.  The  virial  function  is  defined  as 

N  1 

Vi  =  ^  -  mi  ||  cf  i  ||2  (2.89) 

i= 1 

where  Cf,  is  defined  in  Equation  (2.4). 

Proposition  2.3.1  \S2  =  Vj. 

Proof 

We  are  going  to  use  the  induction  method.  Starting  from  N  —  2,  we  have 

Pil  =  \fP l(cf2  —  cn)  .  (2.90) 

But  since  we  already  know  that 


mien  +  m2 cf2  =  0  . 


Then 

mi 

Cf2  = - cn  . 

m2 

Thus  replace  Cf2  in  equation  (2.90)  by  equation  (2.92),  we  get 


_ _ mi  +  m2 

Pil  ~  —\[Pl - Cfl 

m2 


(2.91) 


(2.92) 
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Hence 


'mi(m2  +  mi) 

rn2 


-Cfl 


2  11  Pfl 


mi(m2  +  mi) 


2  mi 


mi  cn 


1 

2 
1 

2mi 

Vj. 


Cfl 


Cfl 


1 2  1  mi 

2  m2 

|2  1  . 
I  +5m2| 


Cfl 


Cf2 


(2.93) 


(2.94) 


Now,  suppose  the  proposition  holds  for  N  particles 


N  1 

induction  hypothesis  E 


N 


N-l  1 

E) 


pH 


(2.95) 


i= 1  i= 1 

we  want  to  show  it  holds  for  (N  +  1)  particles.  Notice  the  addition  of  the 
(N  +  l)th  particle  has  changed  the  center  of  mass  from  q ^  to  q^+1.  By 


dehnition  of  the  Jacobi  vectors  we  get 


Pin  =  VPn+ i(c^li)  -  -  Qc+1)) 


where 


Pn+i  — 


Mn  mN+i 
Mn  +  mjv+i 


where  MN  =  YliLi  m, .  We  also  have 


(2.96) 


(2.97) 


Mjv(?f  -  Qc+1)  +  mjv+ic^ii)  =  0  .  (2.98) 


Thus  equation  (2.96)  can  be  rewritten  as 


PiN 


(Mn  +  mjv+i  )mjv+i 


Mi 


v 


„iV+l 
Uf(iV+l)  • 


(2.99) 
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Cft  and  c^+1  are  also  related  by: 


Therefore 


c£  = 


% 


"+1  +  C »  -  <£ 


—  ^.^+1  i  mN+l  „N+i 

u  +  mn  f(^+1) 


(2.100) 


-S 


N+ 1 


N-1 

E  5 II  f  +  o  II  to 


i=  1 
N 


E* 

2=1 

N 

E 


m  cH 


N  1,2  + 1 II  to  f 


-rrij 


i=  1 


„JV+1  ,  mAr+l„7V+l 
Lfi  “T  -  '  Cr 


Mjv  f(^+1) 


1  (Mjv  +  mjv+Omjv+i  n  ^+1 


N 


i=l 


X!omi  11  C 


Mn 

V+l  II2 

fi  I ! 


"f(iV+l) 


N 


N+l\mN+l  n+1 

Cr 


N 


+ 


l/\^  x'^N+l 

2(I>‘)  yr 

1  m^+1 


*=l 

,w+i 

Jf(Af+l) 


Mat  f(^+1) 


2  1 

+  2m^+i 


,AT+1 

Jf(iV+l) 


2  Iff  AT 


AT 


-mi  ||  c 


2=1 

1 

+  -mAr+i 
N+ 1  . 


,JV+1 

"f(iV+l) 


N+l  2 
fi 


,JV+1 

"f(iV+l) 


1  m 


N+l 


2  Mat 


,jv+i 

■'{(N+i) 


+  - 


1  m 


N+ 1 


2  M, 


N 


,JV+1 

■Jf(AT+l) 


i=l 

V7- 


9  m*  M  Cb 


iV+1  2 
f2 


(2.101) 


Notice  that  we  used  the  fact  Yl+=i  mjC^+1  =  —  mN+iC^+i)-  By  induction 
the  proposition  is  proved.  ■ 
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This  proof  can  be  easily  modified  as  a  proof  to  equation  (2.7).  Hence  it 
must  have  been  known  by  Jacobi. 

The  topology  of  Kendall  shape  space  has  been  partially  classified  in  [16] . 
Some  interesting  special  cases  are  listed  as  follows: 

•  Points  on  a  line:  Ef  =  SN~2(1)  a  unit  sphere. 

•  Points  on  a  plane:  E^  =  CPN~2( 4)  a  complex  projective  space  where 
4  is  curvature  parameter.  Specifically,  E'j  =  *S'2(|). 

•  Higher  dimension:  E™+1  ft;  5'm(m+1)/2~1  where  ft;  means  homeomor- 
phism.  This  is  a  theorem  proved  by  Casson. 

On  Kendall  pre-shape  space,  for  m  >  2,  we  define  a  metric 

d(X,  Y)  =  2arcsin(i  ||  X  —  Y  ||)  =  arccos (tr(XYT))  (2.102) 

where  X ,  Y  6  S^.  It  is  not  difficult  to  see  that  d  is  the  great-circle  metric  on 
the  unit  sphere  S topologically  equivalent  to  the  chordal  metric  inherited 
from  Then  on  Kendall  shape  space  E^[,  we  can  define  a  metric  5 

which  is  the  quotient  metric  of  d. 

6MX),n(Y))  =  min  d(X,  TY)  —  arccos (  max  tr(TXYT))  (2.103) 

TeSO(m)  TeSO(m) 

where  n  is  the  canonical  projection. 
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2.4  Examples 


2.4.1  A  conserved  quantity  of  sequential  pursuit 


In  this  section  we  apply  shape  theory  to  analyze  the  problem  of  sequential 
pursuit.  The  simplest  such  problem  is  of  the  following  form. 

Let  {q\ ,  q2 ,  q-j )  denote  the  positions  of  three  particles  in  lZi .  Suppose  they 
satisfy  the  equations 


q\  =  q-2~  qi 

q2  =  q-s  -  q2 

%  =  q\  -  q.i  ■  (2.104) 

We  want  to  show  that  from  any  initial  configuration,  (cq ,  q2l  q 3)  will  converge 
to  the  centroid  of  the  initial  shape.  This  result  is  known  to  Brocard  in  1877 
[4],  It  was  studied  later  in  [3]  and  more  recently  in  [21], 

The  proof  for  convergence  is  straight  forward  after  we  express  the  system 
equations  using  the  Jacobi  shape  coordinates.  It  is  obvious  that 

q.c  =  +  q2  +  Tj)  =  0  .  (2.105) 


Then  according  to  the  definition  of  the  Jacobi  vectors  in  equation  (2.8) 
Pn  = 


-  qi)  =  =(q3  ~  2g2  +  qi) 


Pi2  ~ 


qs  - 


<12  +  qi 


(« qi  -  93) 


(2.106) 


According  to  the  inverse  transform  presented  in  (2.15),  we  have 

V2  1 


q  1  =  qc 
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<?2 


=  Qc 


V2 

2 

<h  =  Qc  +  ^Pf2 


1 

7e' 


Then,  the  equations  in  Jacobi  coordinates  are 


3  V3 

Pfi  =  —  2^fl  +  ~^f2 
a/3  3 

Pf2  — - ^-Pfl  -  2^f2  • 


(2.107) 


(2.108) 


One  can  see  that  the  system  is  a  linear  system  with  eigenvalues  equal  to 
—  |  ±  i^.  Therefore,  as  t  — >  0,  pn,  Pi2  — >  0.  Since  qc  is  fixed,  the  three 
particles  will  converge  to  qc.  The  eigenvalues  are  invariant  under  the  action 
of  the  democracy  group.  Therefore  the  convergence  does  not  depend  on  the 
choice  for  Jacobi  coordinates. 

Next,  by  using  shape  theory  we  are  to  analysis  the  geometric  property 
of  the  convergence  for  triangular  shapes.  We  want  to  understand  how  the 
shape  of  the  “virtual”  body  formed  by  the  three  particles  evolves  in  time  and 
how  fast  the  whole  body  is  rotating  around  the  centroid.  It  seems  that  these 
questions  have  not  yet  been  answered  in  literature. 

As  shown  in  figure  2.2,  we  establish  a  gauge  convention  as  discussed  in 
section  2.2.  The  origin  are  chosen  to  be  at  the  center  of  mass.  The  rc-axis 
is  aligned  with  the  line  joining  particles  1  and  2  pointing  towards  particle  2. 
We  choose  the  r/-axis  to  be  perpendicular  to  the  x-axis  in  the  plane  formed 
by  the  three  particles  such  that  the  x  —  y  coordinate  system  is  a  right  handed 
system.  We  choose  the  z-axis  to  form  a  right  handed  3-D  coordinate  system 
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Y 


Figure  2.2:  Gauge  convention  for  a  triangular  formation.  The  shape  variable 
Si  is  the  distance  between  particle  1  and  2.  Shape  variable  s2  measures  dis¬ 
tance  between  particle  1  and  3.  Shape  variable  S3  is  the  angle  between  q{q^ 
and  q{c[2- 

with  x  and  y  axis. 

Three  shape  variables  are  chosen.  The  shape  variable  si  is  the  distance 
between  particles  1  and  2.  Shape  variable  s2  measures  distance  between  par¬ 
ticles  1  and  3.  Shape  variable  S3  is  the  angle  shown  in  hgure  2.2.  These  three 
variables  are  enough  to  describe  a  triangle  because  according  to  previous  dis¬ 
cussions,  the  dimension  of  the  Jacobi  shape  space  formed  by  three  particles 
is  3. 

Under  the  chosen  gauge  convention,  the  coordinates  of  particle  2  are 
(si,0,0)r.  Particle  3  has  the  coordinates  (s2  cos(s3),  s2  sin(s3),  0)T.  Thus, 
the  Jacobi  vectors  in  this  body  frame  are 

pi  =  v/pI(si,0,0)T 
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p2  =  v/a^(s2  cos(s3) - p — Si,s2sin(s3),0)T  .  (2.109) 

mi  +  m2 

The  following  calculations  can  be  carried  out: 


II  II2  2 

||  Pi  ||  —  Plsl 

\\p2W2  =  P2((s2Cos(s3) - — — Si)2  +  S2Sin2(s3)) 

m  1  +  m2 

sj  0  0 

Pipf  =  Pi  0  0  0 

0  0  0 

P2P2  = 

(s2  cos(s3)  -  ^^S!)2  s2  sin(s3)(s2  cos(s3)  -  ^2_si)  0 
P2  s2sin(s3)(s2cos(s3)  -  m™m2Si)  s|sin2(s3)  0 

0  0  0 


Thus,  we  can  calculate  the  locked  inertia  tensor  /.  Since 

I  =\\p1\\2e-  pipf  +  ||  p2  ||2  e  -  P2P2  ,  (2.110) 

we  have 

hi  —  P2S2  sin"(s3) 

h  2  =  hi  =  -p2s2sin(s3)(s2cos(s3) - ^ — s3) 

m  1  +  m2 

I22  =  Pi-s2 +  p2(s2cos(s3) - - - si)2 

m  1  +  m2 

^33  =  Pi-s?  +  P2((s2Cos(s3) - ^ - Si)2  +  S2Sin2(s3)) 

m  1  +  m2 

-^23  =  I32  =  1 13  —  hi  —  0  •  (2.111) 
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In  order  to  calculate  the  gauge  potential  A,  the  following  derivatives  are 
calculated: 


Since 


dpi 

dsi 

dpi 

ds2 

dpi 

ds3 

dp2 

dsi 

dp2 

ds2 

dp2 

ds3 


V^[!,0,0]T 
[0,0, 0]T 
[0,0,  Of 


V7£[ 


rn2 

rrii  +  m2 


0,0]r 


v/^[cos(s3),sin(s3),0]T 
VP2[~s2  sin(s3),  s2  cos(s3),  0]r  . 


.  T—  i  /  dpi  u/j2 

Aj  -  I  (pi  x  —  +  p2  x  — 


dp2 


dsi 


dsj 


for  j  —  1,  2,  3  ,  the  results  are: 


Ai  =  p2^-[0,0, — ^ — s2  sin(.s3)]r 

J33  m  i  +  m2 

A2  =  Af2y-[0,  0, - ^ — Sisin(s3)]T 

I 33  mi  +  m2 

.  1  -  2  777-2  /  \ -iT1 

^3  =  [0,0,  s2 - ; - SiS2COS(s3)J  . 

l33  rn  i  +  m2 


(2.112) 


(2.113) 


(2.114) 


We  want  to  find  hi,  the  rotation  speed  of  the  body  and  st  for  i  —  1,2,  3. 
There  are  altogether  six  unknowns.  These  unknowns  can  be  solved  from  the 
six  equations  given  by 


Pi 


fl  X  Pj 


i=  1 


dsi 


(2.115) 
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for  j  =  1,  2.  In  our  context,  we  assume  that  rn i  =  m2  =  m3  =  1.  Therefore, 
when  j  =  1,  according  to  equation  (2.108)  and  equation  (2.115),  the  first 
three  equations  are 


1  c 

1 

d2Sl 

V2 

0  x 

0 

+  Si 

0 

0 

0 

3 

~2Pl  +  ~p2 


V2Sl 

a/3 

s2cos(s3)  -  |si 

3 

/2 

S2  sin(s3) 

2 

0 

l  ^ 
+ 

V  3 

0 

0 

(2.116) 


We  let  =  [fXs,  Qy,  fL]T.  The  above  equations  are  simplified  to 


0 

d2Sl 

-fV^Si  +  -^=  (s2  cos(s3)  -  |si) 

+ 

0 

= 

75^2  sin(s3) 

0 

0 

We  can  then  get  the  following  solutions 


(2.117) 


si  =  -2si  +  s2  cos(s3) 

ily  =  0 

£lz  =  —  sin(s3)  .  (2.118) 

Sl 

On  the  next  step,  we  can  write  the  remaining  three  equations  as 


1 

vT 

cos(s3) 

s2  sin(s3) 

[2 

[2 

0 

+  S2'V5 

sin(s3) 

+S3'n 

s2  cos(s3) 

0 

0 

0 
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(2.119) 


V3 


2  ' Pl  ~  2P2 


3  /  2 


2  V  3 


3  /  2 


s2  cos(s3) 


2  V  3S2  sin(s3) 


One  can  verify  that 


fix  p2  = 


2  sin2(^3) 

3  si 


-  •  —  sin(s3)(s2  cos(s3)  -  §si) 


3  si 


|  s2  sin(s3)fia 


(2.120) 


Then  we  can  observe  that 


fL.  =  0  . 


(2.121) 


The  remaining  two  equations  are  simplified  to 


s2  cos (s3)  -  s3s2  sin(s3) 
s2  sin(s3)  +  s3s2  cos (s3) 

We  find  the  solutions  to  be 


-s2  cos(s3)  -  Si  + 


s|  sin2(s3) 


si 


-s2  sin(s3) 


s 2  sin(s3)  cos(s3) 


Sl 


(2.122) 


s2  =  — s2  -  Si  cos(s3) 

s3  =  (—  )sin(s3)  (2.123) 

S2  Si 

We  now  let 

S\  =  s2  +  s2  +  (s2  +  s^  —  2sis2  cos(s3)) 

S2  =  ^sis2sin(s3)  .  (2.124) 


We  see  Si  is  the  square  sum  of  the  length  of  the  three  edges.  If  we  denote 
the  size  of  the  formation  as  Z  in  Kendall  sense  and  let  Vj  be  the  virial  of 
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the  three  particles,  then  ^/Si  =  \/3  Z  =  \/6Vj.  S2  measures  the  area  of  the 
triangle.  Then 


5*1  =  2(2siii  +  2 s2s2  -  sis2  cos(s3)  -  sis2  cos(s3)  +  sis2sm(s3)s3) 

=  -6(s^  +  s\  -  sis2  cos(s3)) 

=  -3Si  (2.125) 


and 


S2  =  2^lS2  sin(S3)  +  Sl^2  sin(s3)  +  «is2  cos(s3)s3) 

=  -^sis2sin(s3) 

=  -3S2  .  (2.126) 


It  is  obvious  that  Si  — >  0  and  S2  — >  0  as  t  — >  00. 

We  now  prove  that  the  gauge  covariant  angular  speed  T  =  Vt  +  Ylt=i  AA 
is  a  constant.  From  equations  (2.114)  and  the  fact  that  Qx  —  Qy  —  0,  we 
conclude  that  Tx  =  Ty  =  0.  Then 


Yz  —  A  +  AAi  +  A2zs2  +  d3zs3 


S2  .  /  X  2 

J7sm(33)  +  3fe 


-s2  sin(s3)(-2si  +  s2  cos(s3)) 


+  ^si  sin(s3)(s2  +  si  cos(s3))  +  (  s22 


^sis2  cos(s3))(  — 
z  s2 

—  sin(s3)  +  Try— (—  sin(s3)) (sis2  cos(s3)  +  ]-s\  -  si) 

Si  3/33  Si  2 

SiS2  sin(s3) 


£2, 

Si ' 


sin(s3) 


=  6 


S2 


‘33 


A 

9  A  _  A 

Z2  V7 


(2.127) 


56 


This  means  that  the  rotational  speed  is  the  proportion  between  the  area  and 
the  virial  of  the  formation.  We  can  calculate  the  derivative  of  Tz  as 


T 


SiS2  -  S2S! 

Sf 

-S1S2  +  S2S1 


Sf 


0  . 


This  proves  that  T  is  a  constant. 


(2.128) 


2.4.2  Line  formations:  controlling  shape  and  size  sep¬ 
arately 

In  this  subsection  we  try  to  control  formations  of  N  mobile  robots  moving 
along  a  predefined  straight  line.  This  line  can  be  either  the  configuration 
space  of  the  robots  like  a  trail  for  trains  or  a  holonomic  constraint  main¬ 
tained  by  steering  control  in  the  situation  of  keeping  vehicles  in  the  same 
lane  on  a  highway.  In  either  of  the  two  cases,  the  position  of  each  robot 
can  be  described  as  a  scalar.  A  formation  of  such  robots  is  represented  by 
the  relative  distance  between  adjacent  robots.  The  relative  distance  is  often 
of  great  importance  and  need  to  be  adjusted  according  to  safety  standards 
and  communication  requirements.  For  example,  consider  the  case  where  one 
vehicle  is  following  another  on  a  highway,  in  the  presence  of  constant  com¬ 
munication  delays,  the  distance  between  these  two  vehicles  is  increased  when 
the  traveling  speed  is  increased  because  if  the  front  one  has  to  perform  an 
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emergent  stop,  the  back  one  need  a  longer  braking  distance  to  avoid  hitting 
the  front  one.  On  the  other  hand,  the  distance  between  the  two  vehicles 
should  not  be  too  big  to  maintain  the  wireless  communication  link  between 
them.  The  safe  distance  will  also  depend  on  whether  the  back  vehicle  is  a 
truck  or  a  compact  size  car.  Thus,  there  is  an  optimal  distance.  All  the  op¬ 
timal  distances  hence  determine  an  optimal  formation  which  will  satisfy  all 
the  safety  and  communication  requirements.  Here,  we  are  not  going  to  com¬ 
pute  the  optimal  formation.  In  stead,  we  assume  that  the  optimal  formation 
is  known  and  we  want  to  achieve  it  from  an  arbitrary  initial  formation  and 
maintain  it  after  wards. 

Suppose  the  formation  contains  N  vehicles.  The  position  qc  of  the  center 
of  mass  can  be  calculated  according  to  equation  (2.3).  We  can  then  calculate 
the  Jacobi  vectors  pu  by  using  equation  (2.8).  Since  the  configuration  space 
for  each  vehicle  is  1Z.  The  Jacobi  vectors  are  now  scalars.  There  are  N  —  1 
Jacobi  vectors.  Thus  the  pre-shape  space  F  is  1ZN~1 .  Because  the  symmetry 
group  is  trivial,  the  base  space  B  coincide  with  the  pre-shape  space  F .  i.e. 
Pi  =  pu-  This  is  to  say  that  we  do  not  have  to  worry  about  connections  and 
gauge  potentials. 

Let  Sn  denote  the  size  of  the  formation  calculated  according  to  equation 
(2.88).  We  define  s*  =  Pi/Sjsr,  (i  =  1,  2,  ...N  —  2)  as  the  pure  shape  variables. 
It  is  obvious  that  the  Kendall  shape  space  BK  is  a  (N  —  2)  sphere  embedded 
in  B  =  TZn~1. 
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Suppose  the  vehicles  obey  the  simple  equations 


Ui 

Qi  =  — 

ms 


(2.129) 


where  tq  are  speed  controls.  The  system  equations  for  (qCl  )  can  be  derived 
as  follows 


Qc 


Pi 


i=  1 


Vpi(u(i+1) 


EL i  uk 
EL  i  ™k 


(2.130) 


where  (i  =  1,  2, ...,  N  —  1).  Then  the  system  equations  for  the  size  and  pure 
shape  variables  are 


Si 


SN  = 

^\N—1 


Ej= 1  Pi  Pi 
Sn 

Pk^PkPi  PiPk) 

s% 


(2.131) 

(2.132) 


where  (i  =  1,2,  ...,N  -  2). 

Now  we  want  to  find  a  control  law  s.t.  s*  will  be  kept  constant  when  we 
change  the  size  of  the  formation  to  a  non  zero  value  S°.  For  this  purpose  we 
let  Si  =  0.  Hence  one  solution  is 


Pk  Pi 

Pk  Pi 


(2.133) 


for  any  pair  of  k  and  i.  If  this  is  true,  the  derivative  of  the  size  variable  is 


EN  2  • 

_  k= 1  Pk  P 1  rr  P 1 

ON  ~  - 7^ - —  ON - 


Sn  Pi 


Pi 


(2.134) 


A  very  simple  choice  of  control  is  to  let 

Pi  ,  (SN  -  S° ) 

—  =  ~k - ^ - 

Pi  <jjv 


(2.135) 
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for  i  —  1,2,  ...N  —  1.  Then  we  can  drive  Sn  to  S°.  The  controls  Ui  can  be 
calculated  from  (2.130)  and  (2.133).  Notice  that  we  have  N  controls  but  only 
N  —  1  equations.  We  can  add  another  equation  to  either  let  U\  or  qc  be  some 
predefined  speed. 

On  the  other  hand,  we  can  find  a  control  law  to  keep  the  size  constant 
while  making  shape  changes.  Then  from  equation  (2.131)  we  have, 


N—l 

=  0  • 

i=  1 

Then  we  can  represent  ppv-i)  in  terms  of  other  p*  as 


P(N-i)  — 


N—2 

-E 


Pi 


-Pi 


i~i  p(N-V 

The  system  equations  (2.132)  are  then  simplified  as 


(2.136) 


(2.137) 


s 


i 


A 

SN 


(2.138) 


Suppose  we  want  the  controlled  pure  shape  variables  to  trace  geodesics 
on  the  (N  —  2)  sphere.  The  reason  is  that  following  the  geodesics  is  the 
fastest  way  to  change  shape  with  a  given  control  magnitude.  Let  p°  and  p 
denote  two  points  on  the  (N  —  2)  sphere.  Suppose  p°  ^  p,  the  shortest  path 
between  p°  and  p  is  the  intersection  of  the  plane  spanned  by  vectors  p°  and 
p  and  the  sphere  SN~l .  The  tangent  vector  p  of  this  curve  at  point  p  should 
lie  in  the  tangent  space  of  SN~l ,  Hence 


p-p  =  0  . 


(2.139) 
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On  the  other  hand,  this  tangent  vector  p  must  also  lie  in  the  plane  spanned 
by  p°  and  p  i.e. 

p  =  lp°  +  kp.  (2.140) 


Combining  the  above  two  equations  we  can  solve  for  k  and  l  as 


(p-p° 

(p-p) 


Hence,  the  controls  can  be  solved  from 


p  =  Kp°-p 


(p  •  p°) 
(p-p) 


(2.141) 


(2.142) 


and  equation  (2.130)  where  l  >  0  is  free  to  choose.  In  fact,  l  determines  how 
fast  we  want  to  go  along  the  geodesic. 


Proposition  2.4.1  If  we  apply  the  control  law  such  that  (2.142)  is  satisfied, 
then  p  — >  p°  asymptotically  if  the  starting  position  p  —p°. 

Proof  Let 

Vl  =  \  Ih-Plf-  (2.143) 

Then 


VL 


(P  -  P°)  '  P 

Kp  -  P)  ■  (p  -  pL id>) 


i(-  p° 


|2  (P'P1 

1  Up  II 


042 


K 


pII2  +  (p-p°)% 
Ip  II2 


(2.144) 
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According  to  Cauchy-Schwartz  inequality  we  have 


||  P°  ||2  II  P  II2  >  (P'P0)2  -  (2.145) 

Thus  Vl  <  0  where  the  equality  is  true  if  and  only  if  p  and  p°  is  aligned.  In 
our  case,  since  ||  p  ||  =  ||  p°  ||,  the  only  chances  are  p  =  p0  or  p  =  —  p0.  But 
the  configuration  for  p  =  —p°  is  unstable  because  Vl  achieve  its  maximum 
value  there.  Hence  p  — >  p°  is  proved.  ■ 
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Chapter  3 


Feedback  Control  of  Small 
Formations 


Robots  in  a  formation  can  be  viewed  as  particles  subjected  to  control  forces. 
Hence  a  robot  formation  can  be  modeled  as  a  controlled  Lagrangian  system 
of  particles.  What  are  the  interactions,  which  allow  particles  (robots)  to  form 
a  stable  formation,  is  the  problem  we  want  to  investigate. 

A  formation  can  be  studied  as  a  deformable  body.  Its  shape  is  invariant 
under  translation  and  rotation,  and  is  also  independent  of  the  coordinate 
system  in  which  we  choose  to  describe  it.  On  the  Jacobi  shape  space,  the 
global  displacements,  i.e.  translation  and  rotation  for  the  entire  body,  are 
not  present.  As  mentioned  in  the  previous  chapter,  shape  variables  are  rigid 
motion  invariants  such  as  inter-particle  distances,  areas  or  angles  made  by 
lines  connecting  the  particles. 
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In  a  fixed  inertial  frame,  if  the  position  of  each  robot  can  be  determined  by 
using  localization  technology  such  as  GPS,  then  Jacobi  vectors  are  convenient 
to  describe  the  shape  and  orientation  of  the  whole  formation.  A  simple 
controller  is  proposed  in  section  3.2.  Under  such  controller,  we  use  estimates 
of  Jacobi  vectors  as  feedback  to  stabilize  shape  and  orientation  of  a  formation. 

When  no  positioning  system  is  available,  without  knowing  robot  coordi¬ 
nates  in  a  fixed  coordinate  system,  only  shape  variables,  which  are  measured 
using  on-board  sensors  of  robots,  can  be  used  to  control  the  formation.  In 
this  case,  a  gauge  convention  will  be  established  which  provides  a  gauge  frame 
for  any  given  shape  of  the  formation. 

In  a  chosen  gauge  frame,  we  compute  the  controlled  dynamics  of  the 
whole  formation  using  the  Lagrange-D’Alembert  principle.  In  the  resulting 
equations,  control  forces  of  all  robots  are  combined  and  are  represented  as 
controls  for  the  shape,  the  position  and  the  orientation  respectively.  We  are 
now  able  to  design  formation  control  by  starting  from  designing  the  combined 
forces.  The  goal  is  to  compute  control  force  on  each  robot  to  implement  those 
combined  forces. 

Under  the  assumption  that  each  robot  is  able  to  measure  the  shape  and 
rotation  for  the  whole  formation,  we  allow  no  communication  between  the 
robots.  If  all  the  robots  agree  on  a  unified  way  to  establish  a  gauge  frame 
for  the  whole  formation,  a  simple  control  law  proposed  in  section  3.3  can 
be  employed.  In  section  3.5,  we  allow  each  robot  the  freedom  to  attach  its 
own  choice  of  gauge  frame  to  observe  the  whole  formation.  This  requirement 
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often  arises  when  a  certain  coordinate  system  is  more  convenient  than  others 
for  getting  more  accurate  estimates  from  sensory  data.  Geometric  properties 
associated  with  the  Jacobi  shape  space  allow  us  to  utilize  gauge  invariant 
and  gauge  covariant  objects  to  design  a  cooperative  control  law  to  satisfy 
this  requirement. 

3.1  Lagrangian  dynamics  of  formations 

Suppose  a  manifold  M  is  the  configuration  space  of  a  mechanical  system. 
Let  TM  be  the  tangent  bundle  of  the  configuration  space.  If  the  system  is 
Lagrangian,  then  there  exists  a  function  L  on  the  tangent  bundle  TM,  such 
that  the  motion  of  the  mechanical  system  is  described  by  a  set  of  equations 
obtained  by  applying  the  Lagrange-D’Alembert  principle.  We  call  the  func¬ 
tion  a  Lagrangian  function.  Let  ( q,v )  denote  a  point  on  TM  where  q  G  M 
and  v  G  TqM.  Then  the  Lagrange-D’Alembert  principle  asserts  that 

jDvL-DqL  =  i  (3.1) 

where  f  represents  external  force. 

By  defining  Jacobi  vectors  and  then  Jacobi  shape  variables  we  have  gone 
through  a  sequence  of  changes  of  coordinates  on  the  configuration  space  Q 
for  the  entire  formation.  In  the  first  step,  the  transform  is  a  diffeomorphism 
between  Q  and  R3N  mapping  configuration  variables  qi  to  the  position  of  the 
center  of  mass  qc  and  the  Jacobi  vectors  pu  .  In  the  second  step,  the  transform 
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is  a  local  diffeomorphism  between  R3  x  F  and  R3  x  G  x  B  mapping  (qc,  Pu) 
where  i  —  1,  2, (N  —  1)  to  (qc,  g,  Sj )  where  j  =  1,  2,  ...(3N  —  6)  s.t. 

Pii  QPi^Sl)  •••SsN—g)  •  (3.2) 


The  Lagrangian  on  TQ  is  given  by  L(q,q)  =  Ktot(q)  —  V(q).  Before  the 
coordinate  transformation,  Ktot  has  the  form 

1  N 

A'“W)  =  5Em-ll9-l|2  •  (3-3) 

i=  1 

The  Lagrange  equations  for  the  system  are  just  the  Newton’s  equations: 

dV 

miq.i  —  f —  (3.4) 


for  i  =  1,  2, N  where  f)  are  control  forces. 

Apply  the  Lagrange-D’ Alembert  principle  to  Jacobi  pre-shape  space.  The 
Lagrange  equations  for  the  Lagrangian  L(qc,  p{,  qc,  p{)  are 


d  d  L 

dL 

dt  dqc 

A 

dqc 

d  d  L 

dL 

dt  dpi 

dpi  Ui 

(3.5) 

where  uc  is  the  equivalent  force  on  the  center  of  mass  and  Uf  represent  the 
equivalent  control  on  Jacobi  vectors.  Now  the  Lagrangian  function  becomes 


L  =  -M  ||  qc  ||2  +  -  II  Pa  f  ~  V (&,  Pa,  Pf(N-i))  ■  (3.6) 

1=1 

Hence,  we  can  calculate  the  partial  derivatives  in  equation  (3.5)  to  obtain 
the  system  equations  in  Jacobi  coordinates 


Mqc 


dV 

—  o  ^  u 

oqc 


C 
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av 

a - 

Opti 


(3.7) 


Pu  — 


for  i  =  1,2,  ...N  —  1.  We  notice  that  the  control  forces  f)  are  combined  into 
controls  uc  and  Uft. 

We  now  go  one  step  further.  Let  F  denote  the  Jacobi  Pre-shape  space 
coordinated  by  (g,  s )  where  g  G  50(3)  and  s  =  (si,  S2,  ■  ■  ■ ,  s^n-q)  G  B  where 
B  is  the  Jacobi  shape  manifold.  A  point  in  the  tangent  bundle  TF  can  be 
represented  by  (g,  s,  gil,  v)  where  hi  €  so( 3)  and  v  represents  s. 

Let 

A  =  [Ai,  A2, ...,  A^n- 6]  (3-8) 

Then  we  can  rewrite  the  kinetic  energy  in  block  diagonalized  form  as 

Ktot  =  X-M  ||  qc  ||2  +  i(hl  +  As)TI(fl  +  As)  +  ^ stGs  (3.9) 


where 


N—l 


Gjk  =  —AjlAk  +  ^ 


dpi  dpi 
■  i  dsj  dsfc 

i=i  j 


(3.10) 


According  to  the  arguments  in  chapter  2  regarding  the  decomposition  of 
a  tangent  vector  into  horizontal  and  vertical  components,  given  a  tangent 
vector  (fi,  s),  (fl  +  As)  is  its  vertical  component.  Equation  (3.9)  tells  us  that 
the  Riemannian  metric  which  gives  rise  to  Ktot  induces  a  Riemannian  metric 
G  on  the  horizontal  subspace  and  a  Riemannian  metric  /  on  the  Lie  algebra. 

Suppose  V  is  translation  invariant.  Omitting  the  center  of  mass,  we  are 
given  a  Lagrangian  function 

.  .  1 


L—-(Q  +  As)  /(fl  +  As)  +  -s  Gs  —  V (g,  s) 


(3.11) 
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In  order  to  apply  the  Lagrange-D’ Alembert  principle  we  shall  calculate  the 
derivatives 

dL  =  (DgL,  DSL,  D g^L,  DVL) .  (3.12) 

Here  dL  represents  a  one  form  on  the  tangent  bundle  TF.  This  one  form 
will  be  evaluated  at  a  tangent  vector  to  T F.  Let  Y  be  such  a  tangent  vector, 
then  Y  G  TTF .  We  shall  first  compute  Y . 

The  tangent  vector  Y  contains  four  components.  The  first  two  compo¬ 
nents  come  from  taking  Frechet  derivatives  of  the  configuration  variables 
(g,s).  We  use  (gLli,vi)  to  represent  these  two  components.  The  last  two 
components  come  from  taking  Frechet  derivatives  of  the  velocity  variables 
(gLl,  v )  as  follows 

—g  exp(eh2i)(H  +  ew)|6=0  =  g{Llifl  +  w)  ,  (3.13) 

Cl  c 

and 

^-(v  +  ez)\6=0  =  z  .  (3.14) 

Therefore 

Y  =  (gfii,  vi,  +  w),z)  .  (3.15) 

Next,  we  want  to  solve  for  dL  from  the  equation 

dL[Y]  =  jL\t=0  .  (3.16) 

We  compute  the  left  hand  side  of  this  equation  as 

dL[Y ]  =  {DgL,  gLl\)M  +  {DSL,  Vi) 
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^{Dg^L,  g(fi ill  +  w))M  +  ( DVL ,  z)  . 


(3.17) 


Here  the  operation  (  ,  )m  is  the  inner  product  between  matrices  defined  as 

(A,B)M  =  hr(ATB)  .  (3.18) 

We  then  compute  the  right  hand  side  of  equation  (3.16)  as 

■^L\e=0  =  -^-\e=0{^(Q.  + ew  +  A(s +  evi)(s  +  ez))TI(s  +  evi)(Q  + ew 

+H(s  +  eiq)(i  +  cz))  +  —  (i  +  ez)TG(s  +  etq)(i  +  ez) 

-V(g  exp(efii),  s  +  eiq)) 

/  f)A  \  T 

=  wTI(Q  +  As)  +  (  —  [vi,i]  j  J(fi  +  Hi) 

1  d  I 

+— 7— [rq,  +  As,  +  Hs] 

2  os 

1  dC 

+{Az)TI{Vt  +  Hi)  +  -  — [iq,  i,  i]  +  ztGs 
dV 

+-^-fi  +  (DgV,  gil\) m  ■  (3.19) 

Comparing  equation  (3.17)  and  (3.19),  we  first  noticed  that  there  is  only  one 
term  involving  w.  Therefore, 

(Dg^L,  gw) M  =  wTI(tt  +  Hi)  (3.20) 

which  implies 

D/fiL  =  g(I(Cl  +  Asjj.  (3.21) 

Here  we  use  the  notation  (  )  to  denote  the  skew  symmetric  matrix  represen¬ 
tation  of  a  three  dimensional  vector  inside  the  bracket. 
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Next,  there  is  only  one  term  involving  Oi  in  (3.19),  hence 

(DgL,  gQi) m  +  (Dg^L,  g(QiQ)) m  =  (DgV,  gQi) m  (3.22) 
which  implies  that 

DgL  =  gjlin  +  As))  A  +  DgV  .  (3.23) 

Observing  the  terms  involving  z  in  (3.19),  we  obtain 

(. DVL ,  z)  =  (A  z)TI(n  +  As)  +  ztGs  .  (3.24) 

Here,  the  inner  product  is  the  inner  product  between  vectors.  Therefore, 

DVL  =  ATI(Q  +  As)  +  Gs  .  (3.25) 


Finally,  we  gather  the  terms  in  (3.19)  that  contain  v\  and  obtain 
/  qa  \T  i  dl 

(DsL,v  i)  =  f i]  j  J(fl  +  Hi)  + -  — (iq,  fl  +  Hi,  0  +  Hi] 

+  12dl^  +  dI»'-  <3'26> 

In  this  equation  (H,  ill  an(]  AL  are  rank  three  tensors.  We  have 
fr)A  \T 

(—  [ui,s]J  7(0  + Hi)  =  —[v1,s,I(n  +  As)} 

BA 

=  (—)*[s,I(n  +  As),Vl]  (3.27) 

where  ((57)*  is  the  cyclic  transpose  of  .  Similarly, 

dl  dl 

—  [rq,  Q.  +  Hi,  +  Hi]  =  (—)*[£)  +  Hi,  0  +  Hi,  iq]  (3.28) 
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and 


dG 


ds 


[Vl,S,S  j 


S,  S,  Ui]  , 


c.f.  [41]  and  [1].  Therefore,  we  can  solve  for  DSL  as 


(3.29) 


DSL 


r\  A  1  (~)  T 

)*[s,  I(Cl  +  As)]  +  -(^-)*[I2  +  As,  Cl  +  As] 


1  ,dG, 


dV 


2  As  [S’"J+  aF' 


(3.30) 


The  Lagrange-D’ Alembert  principle  tells  us 


d 

DfnL 

DgL 

gua 

dt 

DVL 

DSL 

Us 

Because 


JtDfiL  =  i^(n  +  ^)T) 

—  gCl  (I(Cl  +  As))  +  g—(I(Cl  +  As))  , 


(3.31) 


(3.32) 


we  obtain  the  equation  for  rotation  as 

+  As))  +  gC lJl{Cl  +  As)j-  g(I(Cl  +  As)j Q  -  DgV  =  gug  (3.33) 


which  implies 

—  (J(f2  +  As))  +  [fl,  (J(f2  +  As))  ]^  =  g  lDgV  +  ug  (3.34) 

where  we  use  [ ,  ]/  to  denote  the  Lie  bracket  operation  between  two  matrices. 
In  vector  form,  this  equation  is 

7  OT  T 

— /( Cl  +  As)  =  —Cl  x  I(Cl  +  As)  +  g _1— — f-  ug  .  (3.35) 

kajL/  U 
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Next,  we  have 


YDVL  =  y( ATI(n  +  As)  +  Gs ) 

ILL  LLL 

8  A  d  d 

—  -^^[s,  J(fl  +  As)]  +  AT— (J(f2  +  As))  +  — (Gs)  .  (3.36) 

Therefore,  the  equation  for  shape  changes  are 

ri  d  f)A  f)A 

+*))  +  #&)  =  ({-r--)li,m+Ai)] 

+  2(^7)*^  +  As,  ff  +  As] 

.,,9V 

+2(a7}  [s’s]  +  ~d7  +  Us-  (3-37) 

After  adding  in  the  equation  for  the  center  of  mass,  the  set  of  Lagrange 
equations  takes  the  form 

Mqc  —  uc  (3.38) 

7  ot  r 

—  (I  (SI  +  As))  =  -n  x  I(Sl  +  As)  -  g~1^  +  uq  (3.39) 
dt  og 

d  d  8  A  8  A 

-{Gs)  +  AT-(I(n  +  Ai))  =  ((— )•  -  — )[i. /(Si  +  ^)] 

+  2  [^]*  :  +  As,  ff  +  As) 

(3-40) 


The  relationship  between  the  control  forces  fi5  the  control  on  Jacobi  pre- 
shape  space  (uc,Ufi)  and  the  control  on  Jacobi  shape  space  (uc,ug,us)  comes 
from  a  well  known  fact  for  controlled  Lagrangian  system:  if  the  coordinate 
transformation  r  =  r(q)  is  a  local  diffeomorphism,  which  implies  that 


dr  . 


(3.41) 
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then 


drT 

Ur  =  (~Q^ylu<l  (3‘42) 

so  that  ( ur,f )  =  ( uq ,  q).  Therefore,  from  equation  (2.8),  we  can  calculate  the 
relations  between  (fl5  f2, ...,  fi, ...,  fisr)  and  (uc,  'Ufi**..,  Wfj.,.,-'Uf(jv-i))  as: 


N-l 


ft  i)  crq  ^  ^ 


y^jljUfj 


j=i 


nri\  T  777.2  T  ■  •  •  T  m7- 


(3.43) 


for  i  =  1,  2, ...,  iV  with  the  assumption  that  g0  =  0  and  m0  =  0.  In  the  next 
step,  equation  (2.24)  plays  the  role  of  equation  (3.41).  Then  ug  and  us  can 
be  solved  from 


where  k 


N-l 

"u  X!  Pi  x  "  ' 

3= 1 


1,  2, ...,  (31V  —  6)  and  Uj  =  g  1ujj. 


(3.44) 


3.2  Feedback  law  in  lab  coordinate  system 

If  the  coordinates  of  the  robots  in  fixed  lab  frame  are  available  using  GPS  or 
other  navigation  technique,  due  to  the  structures  in  the  definition  of  Jacobi 
vectors,  we  find  that  the  Jacobi  vectors  can  be  easily  calculated. 

Suppose  each  robot  has  GPS  receiver  on  board  with  its  coordinate  qt 
measured.  Then  we  can  rewrite  the  definitions  for  Jacobi  vectors  in  (2.8)  as 

pf!  =  \fpl{q2  -  q\) 
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Pi2 

Pi3 


\fP2iiM3  ~  Q.2)  + 
\//k}((94  —  Q3)  + 


m  1 


,  -(92  - 

mi  +  m2 
mi  +  m2 

mi  +  m2  +  m3 


9i)) 

(?3  - 


92)  +  -  —  -  (92 

mi  +  m2  +  'm3 


-9i)) 

(3.45) 


The  formula  for  pn  has  similar  form  with  cp  replaced  by  q% . 

Hence,  the  Jacobi  vector  pn  and  its  derivative  pn  will  be  calculated  on  the 
{%  +  l)th  robot.  A  one  directional  communication  link  should  be  established 
between  robot  (i  —  1)  and  i  so  that  robot  (i  —  1)  will  report  (gi_i,gi_i)  and 
(qk  —  qk~  1,  <lk  ~  Qk-i)  where  (1  <  k  <  i  —  1)  to  robot  i.  The  coordinate  of  the 
center  of  mass  qc  and  its  derivative  qc  are  computable  on  the  last  robot. 

The  system  can  be  described  using  system  equation  (3.7).  Suppose  the 
potential  function  V  is  translation  invariant.  Then  we  must  have 

dV 

7T-  =  0  •  (3.46) 

dqc 

Thus  do  not  depend  on  qc.  The  two  equations  in  (3.7)  are  decoupled. 
This  means  the  dynamics  described  by  the  second  equation  is  the  reduced 
dynamics  on  the  tangent  bundle  of  Jacobi  pre-shape  space.  On  this  tangent 
bundle,  consider  a  function  defined  as 

N-l 

VL  =  S^J{hn(pii)  +  -  ||  pu  || 2)  .  (3.47) 

i= 1 

Let  p(]  be  constant  vectors  specifying  the  desired  shape  and  orientation.  The 
functions  hn(z)  satisfies  the  following  conditions 

1.  ^  exists  for  all  z  G  773. 
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2.  ^  =  0  if  and  only  if  z  —  p®. 

3.  Every  ha  is  bounded  below. 


The  derivative  of  this  function  Vl  along  the  trajectory  of  the  reduced 
dynamics  is 

N-l  7  7 

^4  =  y, ("T-”  •  Pfi  +  Pfi  •  Pfi) 

JV-1  m 

E.  ,CUlfi  ..  . 

,.i  Pfi(5y" + Pu) 

\  ^  .  ,  dh,H  dV  . 

—  /  .  Pf*(~  yn  ^ Ufi)  ■ 


We  can  see  by  letting 


“j'  dpu  dpu 


(3.48) 


the  derivative  of  Vl  is 


Ujfi  Pfi  T 


dV  dh 


n 


dpu  d.pn  ’ 


N- 1 


U  =  -5jl|Pfil|2<0. 


(3.49) 


(3.50) 


We  can  apply  LaSalle’s  invariance  principle  to  argue  that  the  controlled  dy¬ 
namics  converges  to  the  maximal  invariant  set  C\  within  the  set  M\  where 
Vl  —  0.  Hence 


Mi  =  {(ft ,pi)  e  TF\p,  =  0} 


(3.51) 


and 


Ci  =  {(pf,Pf)  G  Mi\pf  =  0,4  =  0}  . 


(3.52) 
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Hence,  on  the  set  C\ ,  we  must  have 


dV 

dpu 


0 


i.e. 


dhu 

dpu 


Thus,  we  have  proved  the  following  theorem 


(3.53) 


(3.54) 


Theorem  3.2.1  Suppose  the  Lagrangian  L  is  translation  invariant.  By  us¬ 
ing  the  feedback  control  law  (3.4-9),  the  system  can  be  controlled  to  the  for¬ 
mation  specified  by  the  set  of  Jacobi  coordinates  {p®}  asymptotically. 

Notice  that  the  control  law  iq  is  calculated  on  the  (i  +  l)th  robot  where 
pa  and  pa  are  available  due  to  the  communication  scheme  mentioned  before. 
Now  according  to  equation  (3.43),  the  actual  force  f,+i  on  robot  ( i  +  1)  can 
only  be  computed  if  (rq,  rq+1, ...,  Mjv-i)  are  known.  Therefore,  in  addition 
to  the  communication  link  from  robot  i  to  (i  +  1)  reporting  (gi,g2,  ■■■■,qi)  to 
robot  (i  +  1),  we  need  a  communication  link  from  robot  ( i  +  1)  to  i  reporting 
(ui,  Ui+2-..,  %_i).  Hence,  if  the  messages  are  delivered  in  this  manner,  the 
smallest  time  interval  between  two  consecutive  control  actions  on  robot  1 
would  be  greater  than  twice  the  delay  between  node  1  and  node  N  of  the 
communication  network.  In  addition,  the  form  of  4^-  will  determine  whether 
more  communication  links  are  necessary. 
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3.3  Feedback  law  using  shape  measurements 


The  previous  result  assumes  g*  are  available  in  real  time.  This  assumption 
is  strong  and  not  very  realistic  in  many  applications.  In  some  situations,  a 
GPS  signal  is  not  available.  Even  when  GPS  is  available,  the  measurements 
have  significant  error  in  them  or  the  time  required  to  obtain  an  estimation 
is  significant.  It  will  be  dangerous  for  robots  staying  near  each  other.  On 
the  other  hand,  the  robots  are  usually  equipped  with  on-board  sensors  such 
as  laser  range  finders  which  are  capable  of  measuring  the  mutual  distances 
between  robots  and  angles  between  lines  of  sight.  These  measurements  are 
often  reasonably  accurate.  They  tell  us  the  values  of  the  Jacobi  shape  vari¬ 
ables.  We  show  that  formation  can  be  achieved  by  using  only  these  shape 
measurements. 

3.3.1  Lyapunov-based  control  for  the  general  case 

Suppose  the  potential  function  V  is  translation  and  rotation  invariant  i.e.  V 
is  only  a  function  of  the  shape  variables  s.  Then  in  the  system  equations 
(3.38),  (3.39)  and  (3.40),  the  equation  for  qc  is  decoupled  from  the  other  two. 
Also  we  may  have  ^  =  0.  Then  we  can  define  a  function  on  the  tangent 
bundle  of  the  pre-shape  space  as 

Vl  —  hs(s)  +  —  (fi  +  As)T I( +  As)  +  —sTGs  .  (3.55) 

Let  s°  €  7 Z3N~6  specify  the  desired  formation.  The  function  hs(z)  satisfies 
the  following: 
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1.  exists  for  all  z  G  1Z3N  6  except  for  singular  shapes. 

2.  4k  =  0  if  and  only  if  2  =  s°. 

3.  hs  is  bounded  below. 

The  derivative  of  this  function  along  the  reduced  dynamics  (3.39)  and 
(3.40)  is 

v l  =  (d-^,s)  +  (n,u9)  +  (s,us-^) 

=  {s,us-^-  +  +  (fi,  Ug)  (3.56) 

where  we  use  (  ,  )  to  denote  the  inner  product.  Hence  by  letting  the  control 
law  to  be 


ug 
us 

where  k\  >  0,  we  have 

VL  =  - 

We  can  apply  LaSalle’s  invariance  principle  to  argue  that  the  controlled  dy¬ 
namics  converge  to  the  maximal  invariant  set  C'2  within  the  set  M2  where 
Vl  —  0.  Hence 

M2  =  {( g ,  s,  Q,  s)  G  TF 1 0  =  0,  s  =  0}  (3.59) 

and 

C2  =  {{g,  s,  Q,  s)  G  M2\tl  =  0,  s  =  0}  .  (3.60) 


—kifl 
dV  dh8 
ds  ds 


(3.57) 


-  s 


-A:i  II  fill2  <0. 


(3.58) 
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In  the  system  equations  (3.39)  and  (3.40),  let  14  =  0  and  s  =  0,  we  have 

JI4  =  Ug  =  0 

dV 

Gs  +  m  =  us-  —  =  0  .  (3.61) 

os 

Thus  on  the  set  C'2,  in  order  for  14  =  0  and  s  =  0,  we  must  have 

dhs 

— 1  =  0  .  3.62 

as 

Thus  we  have  proved  the  following  theorem 

Theorem  3.3.1  Suppose  the  Lagrangian  L  is  both  translation  and  rotation 
invariant.  By  using  the  feedback  control  law  (3.57),  the  system  can  be  con¬ 
trolled  to  the  formation  specified  by  the  Jacobi  shape  variable  s°  asymptoti¬ 
cally. 

However,  by  letting  ug  =  —k\Q.  we  already  made  the  assumption  that  14 
can  be  measured.  In  fact,  14  need  not  to  be  measured  accurately.  All  we  need 
is  an  estimation  of  the  direction  of  14  which  will  ensure  ||  14  ||  be  decreasing. 
This  estimation  can  not  be  obtained  by  only  measuring  s  and  s.  Some  extra 
sensors  has  to  be  employed  which  will  observe  the  relative  movements  of  fixed 
landmarks  to  the  formation. 

Given  the  difficulty  of  measuring  14,  we  proceed  to  explore  the  situations 
when  no  estimation  of  14  is  necessary.  Notice  that  in  most  applications  there 
exists  dissipative  forces  such  as  the  frictions  of  ground,  air  or  water.  Suppose 
those  dissipative  forces  can  be  modeled  by  a  Rayleigh  dissipative  function 
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R(q,  q) .  This  dissipation  must  satisfy 

c)  /? 

^<ji  >  0  (3.63) 

dqi 

for  i  —  1,2,  ...N.  The  following  assumptions  are  made: 

1.  This  dissipation  is  translation  invariant. 

2.  After  the  coordinate  change  from  q  to  (qc,  g,  s ),  this  dissipation  function 
can  be  separated  into  two  parts  as 

R(g,  S,  qc,  12,  s)  =  Rc(qc )  +  Rs{g,  S,  o,  s')  .  (3.64) 


3.  Rs  satisfies 

dRs  d  iA 

where  zero  is  obtained  only  if  s  —  0  and  12  =  0. 

4.  If  s  =  0  and  12  =  0,  then  we  have 


dRs 

ds 

dRs 

dn 


0 

0  . 


(3.65) 


(3.66) 


Among  these  assumptions,  the  strongest  one  is  assumption  2.  So  far  we 
only  know 

k  N 

R{g)  =  2  II  ^  II2  (3-67) 

i= 1 

with  k  >  0  satisfies  all  these  assumptions. 

When  such  a  dissipative  function  is  presented,  the  system  equations 
should  be  modified  to 

pjJD 

Mqc  =  uc  -  -A  (3.68) 
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d.r,^  . ...  ^  i  (9K  (9i?s 

—  (/(fi  +  As))  =  —  £1  x  /(n  +  As)  —  g  y—  +  ug  ^y 


(3.69) 


l<Gi> 


d 


,dA w  (9A. 


+  +  ^))  —  ((-^-)*  -  — )[s,  /(n  +  As)] 

1  O  T  1  Of! 

+  o  [■£>“]*  1  +  ^  +  o  [~5 — ]*(-®5 


2  as 
dV 

ds 


2  ds 


aR* 

ds 


(3.70) 


Then  by  using  the  same  Lyapunov  function  as  (3.55)  on  the  tangent  bun¬ 
dle  of  the  pre-shape  space,  the  derivative  of  this  function  along  the  reduced 
dynamics  (3.69)  and  (3.70)  is  now 


•  ,dhs  d  Rs.  ..  dV  dRs, 

Vl  =  {^’s)+{a'u’~xi)  +  {s'u--^--dr) 

dV  dh ,  dRq  .  OR 


=  (s,  u 


5 


(9s 


an 


n . 


Hence  the  control  law  can  be  designed  as 


(3.71) 


", 


= 


=  0 

dV  dha 
ds  ds 


such  that 


V?  = 


dRs  dRs  „ 

-s  -  —An  <  o 


(3.72) 


(3.73) 


ds  an 

We  can  apply  LaSalle’s  invariance  principle.  By  similar  arguments  as  before 
we  can  prove  the  following  theorem 


Theorem  3.3.2  Suppose  the  Lagrangian  L  is  both  translation  and  rotation 
invariant.  Suppose  there  exists  a  Rayleigh  dissipation  satisfying  assumptions 
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1-4 ■  Then  by  using  the  feedback  control  law  (3.72),  the  system  can  be  con¬ 
trolled  to  the  formation  specified  by  the  Jacobi  shape  variable  s°  asymptoti¬ 
cally. 

3.3.2  Various  Lyapunov  functions  and  control  laws 

The  Lyapunov  function  based  control  laws  in  the  last  section  can  be  special¬ 
ized  to  fit  in  different  contexts.  Furthermore,  we  can  use  different  forms  of  hs 
to  avoid  shapes  that  corresponding  to  collision  between  particles  or  collinear 
shapes. 

Suboptimal  distance  function  on  shape  space 

In  [45],  we  let 

h9{s)  =  ^  ||  s  -  s°  ||2  .  (3.74) 

The  resulting  control  law  is  applicable  to  formations  that  contain  an  arbi¬ 
trary  number  of  robots.  However,  one  drawback  of  this  method  is  that  by 
using  ||  s  —  s0  ||,  we  are  using  the  “chordal  distance”.  Hence  the  controlled 
dynamics  is  not  following  the  true  geodesics  on  the  shape  space.  The  algo¬ 
rithm  is  not  the  most  efficient.  In  fact,  the  geodesic  distance  between  two 
points  s  and  s°  on  the  shape  space  B  is 

N—l 

^b(s,s°)  =  min  ||p*(s) -0Pi(s°)  ||2  .  (3.75) 

aeso  3 

i=i 

Generally,  the  minimizer  gmin  is  hard  to  calculate  and  is  hard  to 

compute. 
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Thus,  we  use  a  sub-optimal  but  easier  to  calculate  distance 


N—t 

SB(s,  s°)  =  ^2  II  d*(s)  _  Pi(s° )  |f  •  (3.76) 

i= 1 

Let  hs  (s)  =  \  6b(s,s°).  The  Lyapunov  function  is  now 

1  n-i  ]  1 

Vl  —  —  ^  ||  Pi(s)  —  pi(s°)  ||  +  — (O  +  As)tI(Q  +  As)  +  —stGs  .  (3.77) 

i= 1 

The  derivative  of  this  function  along  the  reduced  dynamics  (3.39)  and  (3.40) 

is 

fln.  fly 

Vl  =  -  pi(s°),-£-s)  +  (n,ug)  +  (s,u8-  -r^) 

fly  fln. 

=  ~  +  J2(-£f)T(Pi(S)  ~Pi(S°)))  +  (Q’Ug)  •  (3-78) 

1=1 

Hence  by  letting  the  control  law  to  be 

ug  =  —kiQ 

fly  N~1  fln. 

Us  =  ~  (^)T(Pi(S)  “  _  *  (3-79) 

i=  1 

where  k\  >  0,  we  have 

VL  =  -  ||s||2  -  h  ||fif  <  0  .  (3.80) 

We  know  that  on  the  tangent  bundle  TF  of  Jacobi  pre-shape  space  F,  Vl  is 
radially  unbounded.  So  we  can  apply  LaSalle’s  invariance  principle  to  argue 
that  the  controlled  dynamics  converge  to  the  maximal  invariant  set  C3  within 
the  set  M3  where  Vl  =  0.  Hence 

M3  =  {( g ,  s,  Q,  s )  G  TF 1 0  =  0,  s  =  0}  (3.81) 
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and 


C3  =  {( g ,  s,  12,  s')  e  M:i\Q  =  0,  5  =  0}  .  (3.82) 

In  the  system  equations  (3.39)  and  (3.40),  let  12  =  0  and  s  —  0.  We  have 


J12  =  Ug  =  0 

Gs  +  IQ  =  us-  —  =  ^2(^)T(pt(s)-pt(s0)).  (3.83) 

i= 1 

Thus  on  the  set  C3,  in  order  for  12  =  0  and  s  =  0,  we  must  have 

N  —  l  r,  . 

Z(£)T(Ms)  ~M^»=0.  (3.84) 

i=l 

The  question  is  now  whether  this  implies  that  Pi(s)  =  Pi(s°)  for  i  =  1,2, ...,  N— 
1. 


We  now  going  to  show  that  if  N  —  3,  the  answer  is  positive.  In  fact,  from 
the  calculations  before,  we  have 


dll  nT 
ds  ’ 


v/pT  0  0 
0  0  0 
0  0  0 


Hence,  let 


dp2  'T 
ds  ’ 


l»2  Q 

mi+m.2 

cos(s3)  sin(s3) 

— s2  sin(s3)  s2cos(s3) 


0 

0 

0 


p2(s)  -  p2(s°)  =  {4,  Sy,  0}T  . 


(3.85) 


(3.86) 


(3.87) 
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We  can  calculate  (^^(piO)  -  Pi(s0))  +  {^§j)T (p2(s)  -  p2(s0))  to  be: 

1712  Sx,  cos (s3)  +  5y  sin(s3)), 

(3.88) 


ds 

0^ 


~~  Sl)  + 


m  i  +  m2 

v/pi(-5xs2  sin(s3)  +  5ys2  cos(s3))}t 


Thus  on  the  invariant  set  the  following  equations  has  to  be  satisfied: 

VFl{Sl-s\)  +  y/jT2  ^  Sx  =  0 

m  i  +  m2 

&x  cos(s3)  +  8y  sin(s3)  =  0 


5xs2  sin(s3)  +  dys2  cos(s3)  =  0 


(3.89) 


Notice  that  if  s2  ^  0,  the  last  two  equations  will  give  Sx  —  0  and  5y  =  0.  Then 
the  first  equation  will  give  =  s°.  Thus  we  proved  that  pi(s)  =  pi(s°)  and 
p2(s)  =  p2 (s°) .  However,  if  s2  =  0,  then  the  second  and  third  particle  collide. 
Thus  proper  initial  conditions  should  be  chosen  so  that  this  case  is  avoided. 
In  other  words,  we  should  choose  the  initial  configuration  sufficiently  close 
to  the  desired  configuration. 

Collision  Avoidance 

For  a  three  particle  formation,  shapes  that  corresponding  to  Si  =  0  or 
s2  =  0  have  to  be  avoided  because  inter  particle  collision  happens  in  these 
two  cases.  Shapes  that  corresponding  to  s3  =  0  shall  also  be  avoided  since 
this  is  the  collinear  shape.  To  achieve  these  goals,  in  addition  to  the  three 
conditions  listed  in  section  3.3.1,  hs(z)  should  satisfy  one  more  condition 


4.  hs(s )  =  oo  when  Si  =  0  for  %  —  1  or  2  or  3. 

Then  under  control  law  (3.79),  the  desired  shape  s°  can  be  established  as¬ 
ymptotically  without  inter-particle  collision. 
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3.4  Cooperative  control  form 


Next,  we  point  out  that  although  controllers  can  be  designed  by  computing 
(ug,us)  first  and  then  find  out  f;,  which  is  the  actual  force  on  each  particle, 
this  procedure  is  gauge  dependent.  This  posts  a  constraint  that  each  robot 
has  to  agree  on  the  same  gauge  convention.  Therefore,  they  have  to  agree 
on  the  measurement  of  gauge  dependent  quantities  such  as  fh  Of  courses, 
because  we  assume  that  each  particle  is  able  to  detect  the  other  particles, 
such  agreements  can  be  enforced.  However,  this  implies  that  some  particles 
have  to  estimate  the  gauge  dependent  quantities  indirectly  from  their  obser¬ 
vations.  Such  indirect  estimates  may  introduce  more  severe  noise  than  direct 
observations. 


5, 


Figure  3.1:  Gauge  convention  for  a  triangular  formation. 


For  example,  in  the  case  of  a  three  particle  formation,  suppose  each  par¬ 
ticle  is  able  to  measure  the  relative  positions  between  itself  and  the  other  two 
particles.  If  all  three  particles  agree  on  the  gauge  convention  established  in 
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section  2.4.1,  as  shown  in  figure  3.1,  then  particle  3  has  to  use  the  relative 
position  between  particle  1  and  particle  2  as  the  estimate  for  rc-axis  of  the 
body  coordinate  system.  This  estimate  incorporates  noise  from  observations 
of  both  particle  1  and  particle  2. 

We  now  proceed  to  argue  that  by  transforming  (ug,  us)  into  a  proper 
gauge  covariant  form,  we  could  have  a  controller  design  procedure  which 
allows  each  robot  to  establish  its  own  gauge  convention.  In  our  example 
above,  for  particle  3,  we  may  establish  the  rr-axis  of  the  body  coordinate 
system  as  being  aligned  with  the  vector  q^q\ ,  which  is  observed  directly  by 
sensors  of  q 3. 

We  define  the  cooperative  control  form  to  be 


Olg  Ug 

cxsk  ^sfc  (^<?)  (3.90) 

for  k  =  1,2,  ...,31V  —  6.  (ctg,as)  defines  a  one-form  on  TF  coordinated  by 
( g ,  s,  g T,  s)  in  the  following  sense 


(ag,  T)  +  (as,  s)  =  ( ug ,  fi)  +  (us,  s)  . 
Then  the  transform  between  Uj  and  (ag,  as)  is 

N- 1 


a 


9 


Olsk 


pj x  uj 

1=1 


N- 1 


E( 


9Pj 

dsh 


Ak  X  Pj)  Uj  . 


(3.91) 


(3.92) 
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In  order  to  show  that  this  transform  is  gauge  covariant,  we  first  show  that 
—  Ak  x  pj)  is  gauge  covariant.  In  fact,  under  a  gauge  transform  such 

that 


pj  =  Ks)p) 

9 1  =  gh(s),  (3.93) 


we  already  know  that  Ak  =  h(s)(Ak  +  7 k).  Then 


-AtXPi  =  +  + 

=  ^ *  d  +  wk  -  x  A 

Qn 1 

=  h(«)(^-4xpj).  (3-94) 

This  shows  that  (|^-  —  Ak  x  p,,-)  is  gauge  covariant.  On  the  other  hand,  Uj 
are  gauge  covariant  because 


=  9  lufj  =  h(s)(g1)  1ufj  =  h(s)u )  . 


(3.95) 


Then 

JV-l 

=  h(s)  p)  x  u)  =  h{s)a\ 

3= 1 
JV_1  So1 

x  p])T/iT(s)/i(s)u)  =  a]k  (3.96) 

3= 1 

Therefore  the  transform  given  by  equation  (3.92)  is  a  gauge  covariant  trans¬ 
form. 

The  cooperative  control  form  allows  us  to  design  (ag,  as)  as  gauge  co¬ 
variant  quantities.  Then  the  actual  forces  fi  where  i  =  (1,2,  ...,N)  can  be 


computed  by  each  member  of  the  formation  in  its  own  coordinate  system 
with  its  own  choice  of  Jacobi  vectors. 

Theorem  3.4.1  Let  robot  1  and  robot  2  be  two  members  of  a  formation 
governed  by  the  controlled  Lagrange  equations  (3.38), (3.39)  and  (3.f0).  Let 
p]j]  and  p“jj  where  j  =  1,  2, (N  —  1)  denote  the  two  different  sets  of  Jacobi 
vectors  chosen  by  the  two  robots.  Suppose  the  two  robots  have  established 
two  gauge  conventions  for  the  formation  that  are  connected  by  the  gauge 
transform  g1  =  g2hT(s )  for  each  orientation  of  the  formation.  Suppose  the 
cooperative  control  form  (ag,al)  computed  by  robot  1  and  (a2,al)  computed 
by  robot  2  are  gauge  covariant,  i.e. 

K,  «s)  =  al)  >  (3-9T) 

and  the  two  robots  agree  on  the  same  set  of  shape  variables  Sk  for  k  = 
1,2, ...,  (31V  —  6)  and  also  agree  on  the  control  forces  for  the  center  of  the 
formation,  which  is  uc.  Then  the  forces  for  each  robot  computed  by  robot  1 
are  identical  to  the  forces  for  each  robot  computed  by  robot  2  i.e. 

f;1  =  f2  (3.98) 

for  i  =  1,  2,  ...N. 

Proof 

From  the  definition  of  (ag,as),  we  have  the  following  equations 

N- 1  N—l 

E  Pj  x  u)  =  h(s)  £  p]  x  v2 

3= 1  3= 1 
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Ar_1  do1  N~1  do 2 

E^  “  A*  x  =  E^  “  ^  X  P ? 


?)T«J 


(3.99) 


According  to  democracy  property  of  Jacobi  vectors,  there  exists  h  e  0(N  — 1) 


[Pill  Pi2i  ■■■i  Pf(iV-l)]  —  [Pfl>  Pi2i  ■■■i  Pf(v-i)]h  > 


(3.100) 


which  implies  that 


9  [Pl,P2>->P(N-l)]  =  9  [Pi,  P2> -'■>  P(N- i)]h 


(3.101) 


Furthermore,  we  can  write 


=  MS)  E  hijPl  ■ 


(3.102) 


According  to  the  definition  of  the  locked  inertia  tensor  /,  we  have 


Efll^-^D 


N—l  N- 1 


Ks)  E  E (hdMp2TP2e  -  P2iPiT))hT{s ) 


i=i  M=i 
N-l 


Ks )  E  5i(P*2TPz2e  -  PipF)hT(s) 


m«)  E  (p*2rp?e  -  PiPiT)hT(s ) 


=  h(s)I2hT(s )  , 


(3.103) 


furthermore, 


JV-i  p  i 

=  (^)_1E^x^ 
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TV- 1  TV— 1 


h(s)(I2)  1hT(s)h(s )  X]  X]  X  (  gsi  +7 kX  PI 


i=l  j,l= 1 


TV-1 


Ms)(/2)  1 x  ( +  7fe  x  ^  ) ) 

iV=i  7 

AT — 1  /  r\  2 

dpi 


KSW2)  ^(p; 


i=i 


M^i+^xpn) 


—  /i(s)(Afe  +  7fc)  ■ 


(3.104) 


Therefore, 


TV— 1 


TV— 1  TV— 1 


X  p)  X  =  X^)  X  hnPi)  x 


i=i 


i=i  *=i 

TV-1  TV— 1 

m«)  X(X^ x  /iT(s)(hoMi1)) 

1=1  j=l 
/TV-1  TV-1 

Ks)  (  X  Pi  X  (X  hy/lT(s)«j) 


2=1 


i=i 


(3.105) 


and 


^-i  /mi 


\~^(dPj  * i  i\T  i 

i=i 


TV-1  TV— 1 

XE^ 

1=1  *=1 


M 

dsk 


+  7fc  x  liq/ij  -  (/Ifc  +  7fc)  x  hiJ-p?)T/iT(s)«J) 


TV-1 


X((§7  ~  x  P?)T(X  h'i//,  ^s)"')) 
fc  1=1 


(3.106) 


2=1 


Equations  (3.105)  and  (3.106)  implies  that 


TV— 1 


«J  = 


hT(s)  X  hPM! 


(3.107) 


2=1 
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which  can  be  written  in  vector  form  as 

[ul,u22,..H.u2(N_  1)]  =  /iT(s)[«i,i4>->M(Ar-1)]hT  (3.108) 

Therefore,  the  controls  u1^  and  uj3  that  corresponding  to  the  Jacobi  vectors 
satisfies 

[■Ufl,  Mf2,  Mf(jv_i)]  =  [Wfi,  Uf2,  •••,  Wf(Ar_i)]h  .  (3.109) 

Then  given  u\  =  u2c,  this  implies  that 

f;1  =  fj2  ,  (3.H0) 

for  i  —  1,2,  ...N.  m 

In  the  next  session,  we  will  show  one  such  control  law  which  stablize  a 
non- singular  formation . 

3.5  Gauge  covariant  control  law 

Suppose  the  potential  function  V  is  invariant  under  translation  and  rotation 
i.e.  V  is  only  a  function  of  the  shape  variables.  Then  in  the  system  equations 
(3.38),  (3.39)  and  (3.40),  the  equation  for  qc  is  decoupled  from  the  other  two. 
Then  we  can  define  a  function  on  the  tangent  bundle  of  the  pre-shape  space 
as 

Vl  =  hs(s)  +  — (Q  +  As)TI(yt  +  As)  +  —sTGs  (3.111) 

where  s°  specifies  a  desired  shape.  The  derivative  of  this  function  along  the 
reduced  dynamics  (3.39)  and  (3.40)  is 

■  ,dhs  ,  dV , 

Vl  =  ^~ds'S^  +  +  ^S)Us  ~  ~ds^ 
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(3.112) 


dV  dhs.  .  . 

=  97  +  ^-)  +  <«-“»> 


where  we  use  (  ,  )  to  denote  the  inner  product. 

In  [45],  we  showed  that  by  letting  the  control  law  be 


ug  =  —  kifl 

dv  f  o,  • 

Ms  =  -W - (s  -  S  )  -  S 

OS 

where  k\  >  0,  we  have 

VL  =  -  ||  s  ||2  —  k\  ||H||2  <  0 

and  the  control  law  will  stabilize  the  shape  s  =  s°  and  hi  = 
we  can  see  here  this  control  law  is  not  gauge  covariant. 

We  now  design  a  new  cooperative  control  law  to  be 


ag  =  —kT 

dV  dhs  ,  . 

Qts  TT  — " ks 

as  ds 


which  is  obviously  gauge  covariant.  The  resulting  (ug,us)  is 


ug  =  —k(Q  +  As) 

dhs  i  aT/^  a  \ 

us  =  — - —  —  ks  —  kAT (Vt  +  As)  . 

os  ds 

Then,  under  such  a  control  law,  we  have 

Vl  =  —  ||  s  ||2  —  k  ||  Q  +  As  ||2  <  0 


with  Vl  —  0  if  and  only  if  hi  =  0  and  s  =  0 


(3.113) 

(3.114) 
.  However,  as 

(3.115) 

(3.116) 

(3.117) 
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Now  we  can  apply  LaSalle’s  invariance  principle  to  argue  that  the  con¬ 
trolled  dynamics  converge  to  the  maximal  invariant  set  C2  within  the  set  M2 
where  Vl  =  0.  Hence  let 


M2  =  {( g ,  s,  0,  s)  G  TF\Q  =  0,  s  =  0} 

(3.118) 

and 

C2  =  {(<7,  s,  O,  s)  e  M2  =  0,  s  =  0}  . 

(3.119) 

In  the  system  equations  (3.39)  and  (3.40),  letting  H  =  0  and  s 

=  0,  we  have 

=  Ug  =  0 

dV 

G's  +  Ifl  =  us  — —  =  0  . 

os 

(3.120) 

Thus  on  the  set  C2,  in  order  for  H  =  0  and  s  =  0,  we  must  have  ^  =  0. 
Therefore,  we  have  proved  the  following  theorem 


Theorem  3.5.1  Suppose  the  potential  V  is  rigid  motion  invariant.  By  using 
the  cooperative  feedback  control  law  (3.115),  the  Jacobi  shape  s°  is  locally 
asymptotically  stabilized. 

Again,  by  letting  ag  =  —k(Q  +  As),  we  already  made  the  assumption 
that  O  can  be  measured.  In  fact,  all  we  need  is  an  estimate  of  the  direction 
of  T  which  will  ensure  ||  T  ||  be  decreasing. 

3.6  Simulations  and  results 

We  design  simulations  to  verify  the  gauge  covariant  control  law  and  theorem 
3.4.1.  In  MATLAB,  we  model  each  robot  as  a  Newtonian  particle  with  unit 
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mass  subject  to  a  control  force.  We  show  here  the  simplest  example  of  a 
formation  of  three  particles.  Since  the  formation  is  always  triangular,  the 
experiment  can  be  carried  out  in  a  plane  if  we  choose  the  initial  velocity  for 
each  particle  to  stay  in  a  plane.  The  following  table  shows  the  initial  position 
and  velocity  of  the  three  particles. 


Particle 

Position 

Velocity 

1 

(0,0) 

(2,1) 

2 

(-2, -3.5) 

(-U) 

3 

(0.1, -0.5) 

(-1,1) 

We  choose  a  set  of  three  shape  variables  as  in  section  2.4.1.  Let  si  be 
the  distance  between  particle  1  and  2.  Shape  variable  s2  measures  distance 
between  particle  1  and  3.  Shape  variable  S3  measures  the  angle  as  shown 
in  Figure  3.1.  The  desired  shape  of  the  formation  is  an  equilateral  triangle 
described  by 

si  =  s2  =  2  ,  S3  =  —  .  (3.121) 

In  experiment  1,  all  particles  establish  the  same  gauge  frame  as  in  section 
2.4.1.  They  also  choose  the  same  set  of  Jacobi  vectors  as  shown  in  Figure  3.1. 
We  assume  no  noise  in  the  measurements  of  the  shape  variables.  Under  the 
gauge  covariant  control  law  (3.115),  the  formation  converges  to  the  desired 
shape  as  shown  in  Figure  3.2.  The  convergence  of  the  three  shape  variables 
are  shown  in  Figure  3.3(a),  3.3(b)  and  3.3(c).  The  rotation  speed  of  the  for¬ 
mation  are  shown  in  Figure  3.3(d).  We  see  that  the  formation  stops  rotating. 
On  the  other  hand,  the  center  of  mass  is  moving  constantly  at  velocity  (0, 1). 
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Figure  3.2:  Experiment  1,  motion  and  shape  changes  of  the  three  robot  for¬ 
mation  when  all  robots  agree  on  the  same  gauge. 


In  experiment  2,  particle  1  and  2  still  use  the  same  gauge  frame  as  in  the 
first  experiment.  For  particle  3  we  establish  a  different  gauge  frame  as  shown 
in  Figure  3.4.  We  also  choose  a  new  set  of  Jacobi  vectors,  with 


Pn  = 
Pi2  = 


92) 

<?2  +  <?3 


)■ 


(3.122) 


We  use  the  same  gauge  covariant  control  law  as  in  experiment  1.  The  for¬ 
mation  also  converges  as  shown  in  Figure  3.5.  We  see  there  is  no  difference 
between  Figure  3.5  and  3.2.  This  is  also  true  when  we  compare  the  con¬ 
vergence  plots  for  the  shape  variables.  This  validates  the  claims  in  theorem 
3.4.1. 
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2.4 

2.2 


0.6  - 
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(a)  changes  in  si 


(b)  changes  in  S2 


(c)  changes  in  S3  (d)  rotation  speed 

Figure  3.3:  Convergence  of  shape  variables  and  rotation  speed  when  all  robots 
agree  on  the  same  gauge. 
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Figure  3.4:  The  gauge  frame  and  Jacobi  vectors  for  robot  3. 
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Figure  3.5:  Experiment  2,  motion  and  shape  changes  of  the  three  robot  for¬ 
mation  when  robot  3  uses  different  gauge  frame  from  robot  1  and  2. 
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(a)  changes  in  si 


(b)  changes  in  s2 
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(c)  changes  in  S3  (d)  Rotation  speed 

Figure  3.6:  Convergence  of  shape  variables  and  rotation  speed  when  robot  3 
uses  a  special  gauge. 
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Next,  we  compare  the  performance  of  the  gauge  covariant  control  law 
when  there  are  simulated  noise  in  the  shape  measurements.  In  our  simulation, 
each  particle  is  able  to  determine  the  relative  positions  of  the  other  two 
particles.  We  add  independent  Gaussian  noise  A/”(0,  0.1)  in  the  estimates 
for  both  the  x  and  y  coordinates  for  the  relative  positions.  Hence  noise  is 
introduced  in  the  estimates  for  the  shape  variables  and  the  rotational  speed. 
If  particle  3  uses  the  same  gauge  as  particle  1  and  2,  it  has  to  estimate  vector 
q{q2  in  order  to  estimate  the  rotational  speed.  If  particle  3  uses  the  special 
gauge  as  shown  in  Figure  3.4,  it  needs  to  estimate  q2q 3.  The  estimate  for 
q{q2  is  more  noisy  than  the  estimate  for  q^q-.i ■ 

To  compare  the  results  for  the  two  cases,  namely  the  case  of  same  gauge 
frames  and  the  case  of  different  gauge  frames.  We  run  100  experiments  in 
each  case.  We  then  compare  the  mean  and  variance  of  the  values  for  the  shape 
variables  and  the  rotational  speed  in  the  last  1.5  seconds  of  the  simulation 
when  “steady  state”  is  reached.  As  shown  in  Figure  3.8,  3.9  and  3.10,  using 
different  gauge  frames  slightly  outperforms  using  same  gauge  frames.  But 
when  its  comes  to  the  rotational  speed,  using  different  gauge  frames  has 
a  clear  advantage.  The  variance  of  the  rotational  speed  when  same  gauge 
frames  are  used  is  much  larger  than  the  variance  of  the  final  rotational  speed 
when  different  gauge  frames  are  used  as  shown  in  Figure  3.7. 
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(a)  Mean  (b)  Variance 

Figure  3.7:  The  mean  and  variance  of  the  rotation  speed  in  the  last  1.5  seconds 
of  the  simulation.  Red  solid  line  represents  the  case  when  all  robots  use  the 
same  gauge.  Blue  dotted  line  represents  the  case  when  robot  3  chooses  a 
special  gauge. 


(a)  Mean  (b)  Variance 

Figure  3.8:  The  mean  and  variance  of  s i  in  the  last  1.5  seconds  of  simulation. 
Red  solid  line  represents  the  case  when  all  robots  use  the  same  gauge.  Blue 
dotted  line  represents  the  case  when  robot  3  chooses  a  special  gauge. 
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(a)  Mean  (b)  Variance 

Figure  3.9:  The  mean  and  variance  of  s 2  in  the  last  1.5  seconds  of  simulation. 
Red  solid  line  represents  the  case  when  all  robots  use  the  same  gauge.  Blue 
dotted  line  represents  the  case  when  robot  3  chooses  a  special  gauge. 


(a)  Mean  (b)  Variance 

Figure  3.10:  The  mean  and  variance  of  S3  in  the  last  1.5  seconds  of  simula¬ 
tion.  Red  solid  line  represents  the  case  when  all  robots  use  the  same  gauge. 
Blue  dotted  line  represents  the  case  when  robot  3  chooses  a  special  gauge. 
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Chapter  4 


Navigation  in  Plane 


In  an  unknown  environment,  a  moving  robot  has  to  be  able  to  avoid  collision 
in  order  to  survive.  Because  the  environment  might  be  changing,  we  install 
various  sensors  on  the  robot  so  that  it  is  able  to  observe  its  surroundings. 
The  behavior  of  the  robot  should  be  designed  to  react  to  the  observations. 
An  algorithm  of  such  kind,  if  it  enables  the  robot  to  arrive  at  a  destination 
without  collision,  is  called  a  navigation  algorithm.  Currently,  when  a  map  is 
unavailable,  there  are  two  major  categories  of  navigation  algorithms. 

The  first  category  is  potential  function  (or  navigation  function)  based  al¬ 
gorithms  as  introduced  in  [18],  [28],  [33]  and  various  other  papers.  A  robot 
follows  the  gradient  vector  held  generated  by  some  artificial  potential  func¬ 
tion  assigned  to  the  workspace.  The  potential  function  assumes  its  minimum 
at  the  destination,  and  detection  of  new  obstacles  will  cause  a  recalculation 
of  the  function.  This  approach  is  computationally  efficient  and  easy  to  imple- 
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ment.  Furthermore,  incorporating  dynamic  model,  the  algorithm  produces 
an  explicit  control  law  for  the  robot.  But  it  has  two  drawbacks.  First,  the 
potential  function  will  almost  certainly  have  critical  points  other  than  the 
goal.  Instead  of  always  moving  toward  the  goal,  the  robot  might  be  trapped 
near  such  a  critical  point.  Second,  the  path  may  be  hard  to  compute  and 
predict.  This  is  due  to  the  complexity  of  the  potential  functions,  and  the 
difficulty  to  integrate  a  gradient  vector  field. 

The  second  category  of  navigation  algorithms  use  local  path  planning, 
that  is,  the  technique  to  prescribe  a  path  according  to  current  sensor  read¬ 
ings.  A  typical  case  is  the  “bug”  family  originated  from  the  work  of  Lumelsky 
and  Stepanov[24],  where  a  robot  is  instructed  to  follow  boundary  curves  of 
obstacles.  It  is  proved  that  by  following  the  boundary  curves  until  certain 
conditions  are  satisfied,  and  then  departing  for  the  goal,  a  robot  is  guaran¬ 
teed  to  reach  the  goal  without  collision.  Of  course,  in  addition  to  navigation, 
we  want  the  path  traveled  by  the  robot  to  be  the  shortest.  Some  interesting 
results  on  the  optimality  of  local  path  planning  is  addressed  in  [34],  Com¬ 
paring  to  algorithms  based  on  navigation  functions,  the  local  path-planning 
algorithms  always  predict  coherent  paths.  However,  they  do  not  tell  the  ro¬ 
bot  how  to  follow  such  a  path.  One  has  to  solve  an  inverse  dynamics  problem 
to  find  an  applicable  control  law.  This  process  is  difficult  and  a  solution  is 
not  guaranteed. 

Because  it  is  very  difficult  to  avoid  the  unwanted  critical  points  of  the 
navigation  functions,  a  recent  development  is  to  keep  the  robot  at  a  constant 
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speed.  Control  will  only  be  applied  to  steering.  With  the  constant  speed 
assumption,  such  a  control  law  has  a  gyroscopic  nature  [42],  The  authors 
of  [37],  [38]  and  [44]  give  empirical  ways  to  design  steering  control  laws  for 
obstacle  avoidance,  but  the  methods  are  still  in  their  preliminary  stages  and 
are  only  applied  to  a  limited  number  of  special  cases.  In  [32],  the  authors 
proposed  a  navigation  steering  law  with  obstacles  treated  via  potential  func¬ 
tions  on  the  space  of  steering  angles.  This  paper  has  inspired  the  group  led 
by  Warren  to  investigate  the  steering  behavior  of  human  beings  in  a  non¬ 
static  environment,  see  [9]  and  see  [10]  for  the  latest  results.  This  approach 
is  promising  when  the  obstacles  are  relatively  small  compared  to  the  aperture 
of  the  range  sensors. 

We  believe  boundary  curves  of  the  obstacles  provide  cues  for  designing 
steering  vector  fields.  In  a  plane,  an  oriented  boundary  curve  is  determined 
by  its  curvature  function  in  the  canonical  Frenet-Serret  frame  [26]  together 
with  the  location  of  one  point  on  the  curve.  The  curvature  function  can  be 
estimated  from  the  range  data  obtained  by  sensors  such  as  sonar  or  laser 
range  finder  (ladar).  By  using  such  curvature  measurements,  we  have  given 
a  systematic  way  to  produce  gyroscopic  steering  vector  fields  which  guar¬ 
antee  avoiding  obstacles  and  reaching  the  goal.  We  take  advantage  of  new 
developments  in  shape  theory  [45]  and  formation  control  [14],  The  essence  is 
that  we  not  only  produce  a  justifiable  control  law,  but  also  predict  paths  for 
the  robot. 
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4.1  Frenet-Serret  system  for  planar  curves 


With  a  fixed  coordinate  system  being  given  in  a  plane,  any  point  in  the  plane 
can  be  denoted  by  using  a  vector  r.  If  a  point  is  chosen  as  the  starting  point 
of  a  planar  curve,  then  any  point  on  the  curve  can  be  represented  by  r(s) 
where  s  is  the  length  of  the  curve  measured  between  the  starting  point  and 
the  current  point.  The  derivative  if  well-defined,  is  called  the  tangent 
vector  at  s.  A  regular  curve  is  a  curve  with  well-defined  tangent  vectors  for 
all  values  of  s. 

We  can  then  attach  a  moving  coordinate  frame  to  a  regular  curve.  For 
any  point  on  the  curve,  we  put  the  origin  of  the  moving  frame  at  the  point, 
the  x-axis  will  be  aligned  with  the  tangent  vector  to  the  curve  and  the  y- axis 
will  be  perpendicular  to  x-axis  but  form  a  right  handed  coordinate  system 
with  it. 

To  avoid  confusion,  we  denote  the  axes  of  the  fixed  coordinate  system 
as  X  and  Y .  We  now  define  vector  x  as  the  unit  tangent  vector  along  the 
x-axis  and  define  vector  y  as  the  unit  vector  along  the  y- axis.  We  can  then 
study  the  motion  of  three  vectors  r,  x  and  y  in  the  fixed  coordinate  frame. 
Frenet  and  Serret  discovered  a  slightly  different  but  equivalent  version  of  the 
following  set  of  equations: 


dr 

ds 

dx 

ds 

dy_ 

ds 


x 

K(s)y 
— k(s)x  . 


(4.1) 
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Here,  k(s)  is  called  the  algebraic  curvature  of  the  curve. 

Because  the  vectors  x  and  y  are  orthogonal,  if  we  construct  a  matrix  by 

x  y  r 

9  =  ,  (4.2) 

0  0  1 

then  g  G  SE( 2).  Equation  (4.1)  now  has  a  simpler  representation  as 

j-  =  9?,  (4.3) 

as 

where 

0  —k(s)  1 

£  =  k(s)  0  0  (4.4) 

0  0  0 

and  £  G  se( 2),  the  Lie  algebra  associated  with  SE( 2).  Thus,  a  regular  curve 
in  the  plane  can  be  studied  as  a  curve  on  Lie  group  SE( 2)  determined  by 
equation  (4.3). 

4.2  Bertrand  family 

There  exists  a  family  of  straight  lines  that  are  parallel  to  any  given  straight 
line  in  the  plane.  Choosing  one  in  this  family  as  reference,  on  each  side  of  the 
reference,  we  can  determine  any  family  member  by  knowing  its  distance  to 
the  reference  line.  As  generalization  to  this  well-known  setting,  for  a  regular 
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curve  determined  by  (<?o(so),  fto(so)),  where 


So (so) 


x0(s0)  yo(s0)  r0(s0) 


0  0 


1 


(4.5) 


we  may  construct  a  family  of  curves  that  are  “parallel”  to  it. 

We  first  find  the  starting  point  for  each  family  member.  Let  So  =  0.  For 
each  value  of  A,  there  exists  a  unique  group  element  g\$  €  SE( 2),  satisfying 


So  1(0)(sao) 


1  0  0 
0  1  A 
0  0  1 


We  can  solve  g\ 0  as 


(4.6) 


5ao 


xo(0)  y0(0)  r0(0)  +  Ay0(0) 
0  0  1 


(4.7) 


On  the  next  step,  we  construct  one  “parallel”  curve  starting  from  the 
initial  point  given  by  g\o .  Notice  that  curves  starting  from  those  g\o  where 
A  >  0  are  on  one  side  of  curve  g0(so )  while  curves  starting  from  those  g\0 
where  A  <  0  are  on  the  other  side.  When  A  <  0  and  k0  <  0,  we  require  that 


A  > 


kol ' 


(4.8) 


When  A  >  0  and  k0  >  0,  we  require  that 

1 


A  < 


K\  ' 


(4.9) 
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This  is  to  ensure  1  —  kq\  >  0  so  that  the  tangent  vector  is  always  well-defined. 
For  each  A,  there  exists  a  well  defined  smooth  function  sa(sq)  s.t. 


sa(0)  =  0 

^  =  1-«0A.  (4.10) 

dso 

Then  since  1  —  >  0,  the  inverse  function  exists  so  that  So  is  also  a  function 

of  s\.  Such  function  s\  is  called  a  reparametrization  of  the  curve.  We  then 
let 


Ka(-Sa) 


ftp(5o) 

1  —  KqX 


(4.11) 


A  curve  on  SE( 2)  can  be  constructed  by  the  following  differential  equation 


dg\ 

ds) 


=  9  a 


0  ~K\  1 

0  0 
0  0  0 


(4.12) 


Definition  4.2.1  Given  a  planar  curve  described  by  (go(so),  fto(so));  for  each 
value  of  A  >  0,  suppose  we  can  construct  a  curve  (g\(s\),  K\(s\))  starting 
from  g\o  and  satisfies  equation  (f-12),  with  s\  given  by  equation  (f.10)  and 
K\  given  by  equation  (4-11).  We  define  curve  (g\(s\),  K\(s\))  as  a  Bertrand 
mate  to  curve  (go(so),  k0(s0)).  The  two  curves  form  a  Bertrand  pair.  And 
the  family  of  all  Bertrand  mates,  indexed  by  A,  is  defined  as  a  Bertrand 
family  for  curve  (go(so),  fto(so))-  We  say  (go(so),  «o(so))  is  the  reference 
curve  of  its  Bertrand  family. 

We  can  re-parametrize  all  members  of  a  Bertrand  family  by  Sq.  By  using 
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the  chain  rule,  from  equation  (4.12)  and  equation  (4.10),  we  obtain 


dgx 

ds0 


dgx  dsx 

ds\  ds0 

\  0 


g  a 


«o(so) 

0 


— Ko(so)  1  —  «oA 
0  0 


0 


0 


(4.13) 


Let 


0  '-'«o (*o)  1  -  «oA 


£(so,  A) 


Ko(«o)  0  0 

0  0  0 


(4.14) 


Then,  any  given  value  of  so  determines  a  unique  group  element  gx(so)  located 
on  the  Bertrand  mate  indexed  by  A.  The  relative  displacement  between  gx(so) 
and  r/0(so)  is 


hx(s0)  =  g0  \s0)gx(s0 )  . 


(4.15) 


Theorem  4.2.2  (Bertrand)  Iix(sq)  is  a  constant  group  element  of  SE (2) 
for  all  s0  >  0  i.e. 


hx(s0) 


1  0  0 
0  1  A 
0  0  1 


(4.16) 


Proof 

According  to  the  definition  of  Bertrand  mate,  we  know  that  when  so  =  0, 
h\( 0)  satisfies  (4.16)  .  We  now  compute  the  derivative  of  hx(s0)  with  respect 
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to  So- 


dhx(s0) 

ds0 


ijo1  ,  - 1 


(o  dso 

_  i  dg  o  _  i  _  i  c 

9o  l—9o  9x  +  9o  9x^\ 

CLSc) 


(4.17) 


If  h\(so)  takes  the  form  in  equation  (4.16)  ,  then  by  direct  calculation  we 
can  show 


dhx(so) 

dsn 


=  0 


(4.18) 


Because  this  is  true  for  all  values  of  so  we  conclude  that  h\(so)  =  h\(0)  for 
all  values  of  s0.  ■ 

The  following  corollary  is  obtained  by  studying  the  components  of  the 
matrix  representation  of  h\. 


Corollary  4.2.3  The  following  properties  hold  for  all  values  of  s0: 

1.  x0  ■  y a  =  0  and  x0  ■  xA  =  1  . 

2.  rA  -  r0  =  Ay0. 

3.  ||  r0  —  rA  ||  =  A. 

The  purpose  for  us  to  study  these  properties  is  to  apply  them  to  formation 
control  and  navigation  for  particles  with  constant  speed. 


Ill 


4.3  Shape  space  for  particles  with  constant 
speed 

In  many  applications,  formation  members  are  required  to  move  at  constant 
speed.  For  example,  for  unmanned  aerial  vehicles  (UAVs)  and  high  speed 
ground  vehicles,  keeping  constant  speed  improves  fuel  efficiency  and  dynamic 
stability.  We  call  a  particle  with  constant  speed  a  vehicle. 

From  now  on,  we  assume  that  all  our  particles  have  unit  mass.  In  a  plane, 
the  motion  of  a  Newtonian  particle  r  G  R2  can  be  described  as  r  =  f.  If  the 
particle  has  speed  u  and  wants  to  keep  the  constant  speed,  we  require 

rx+ry  =  v 

4  rx  +  iy  fy  =  0.  (4.19) 

By  introducing  a  new  variable  9  and  defining  u  =  9,  we  can  write  down  the 
solution  as 


rx  =  v  cos  6 

fy  =  v  sin  9 

9  =  u.  (4.20) 

And  we  must  let  f  be 

4  =  -fy  u 

iy  =  fxu.  (4.21) 
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Notice  that  f  is  always  perpendicular  to  the  velocity  vector  r.  Therefore, 
it  will  not  change  the  kinetic  energy  of  the  particle.  We  say  the  force  is 
gyroscopic.  Under  such  a  force,  the  particle  lives  on  a  submanifold  of  its 
phase  space  where  kinetic  energy  is  constant.  This  submanifold  is  SE( 2).  On 
this  submanifold,  The  states  of  the  particle  can  be  represented  by  (rx,ry,6) 
and  the  force  can  be  simplified  to  a  steering  control  which  is  represented  by 
u. 


Jacobi  shape  theory  is  difficult  to  be  applied  to  vehicles  without  modifi¬ 
cation  and  extension.  We  establish  a  shape  theoretic  approach  for  formation 
control,  which  is  based  on  [14].  This  theory  will  then  be  applied  to  create 
boundary  following  behaviors  for  vehicles. 

Given  (N  +  1)  vehicles,  for  i  —  0, 1,  2, ...,  N,  define 


Xi  = 


U  = 


cos  6t 
sin  9i 

Xi 

Vi 


yi  = 


—  sin  6i 
cos  6 ; 


9i  = 


(4.22) 


(4.23) 


Then  define 

r 

Xi  yi  r; 

0  0  1 

x;  and  yi  define  a  frame  carried  by  particle  i.  The  motion  of  particle  can 
now  be  expressed  as 


r  i  =  iy  Xi 
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Vt  yi  Ui 


X;  = 

y;  =  Vt  X;  ut  .  (4.24) 


Comparing  equation  (4.24)  with  equation  (4.1),  we  notice  that  the  motion 
description  of  a  unit  speed  particle  is  the  same  as  a  planar  curve,  with  the 
curvature  n  replaced  by  the  steering  control  Similarly,  the  vehicle  motion 
can  also  be  described  by 

9i  =  9iti  (4-25) 


where 


0  -ViUi  Vi 


v.i  Ui  0  0 


0  0  0 


(4.26) 


We  select  the  vehicle  go  as  the  reference  vehicle,  then  define  the  shape 
variables  to  be  h,  =  g^gi  for  i  =  1,  2, ...,  N.  The  space  of  each  hi  is  SE( 2). 
The  shape  space  of  N  +  1  vehicles  is  the  product  of  N  copies  of  SE{ 2). 

In  this  section,  we  extend  these  ideas  so  that  they  are  able  to  be  applied 
to  controller  design  for  boundary  following.  We  only  require  the  reference 
vehicle  to  travel  at  constant  speed.  For  other  particles,  their  speed  vt  are 
not  constant.  Later  we  will  see  that  in  the  boundary  following  problem,  vt 
depend  on  Uq,  the  steering  control  of  the  reference  vehicle. 

The  shape  dynamics  is  described  by  the  system  equations  for  the  shape 
variables  hi.  We  compute  the  shape  dynamics  as 


hi  =  %19i  +  9o19i 
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(4.27) 


—  9o  9o9o  9i  +  9o 

—  — £o  hi  +  hi£i  . 

We  would  then  like  to  characterize  the  equilibria  of  the  shape  dynamics. 
The  following  theorem  tells  us  that  a  relative  equilibrium  of  the  shape  dy¬ 
namics  of  a  pair  of  particles,  one  traveling  at  constant  speed  and  the  other 
one  not,  produces  trajectories  that  form  a  Bertrand  pair. 

Theorem  4.3.1  Let  g\{t)  and  go(t)  describe  the  motion  of  two  particles  in 
the  plane  parametrized  by  t  satisfying 

9\  =  9i£i 

9o  =  9o£o  ■  (4.28) 

Suppose  vehicle  described  by  go  travels  at  unit  speed.  Let  h\(t )  =  gQl(t)gi(t) . 

Then  h\{t)  satisfies  the  differential  equation 

hi  =  — +  h i £ i  .  (4.29) 

Furthermore,  if  this  shape  dynamics  has  equilibria,  then  we  have  the  following 
conclusions: 

1.  hi(t)  =  he(t)  =  h\  are  equilibria  of  the  shape  dynamics,  where  h\  are 
constant  group  elements  given  in  equation  (4-16). 

2.  If  the  steering  control  of  go(t)  is  not  a  constant,  then  hi(t)  =  he(t)  =  h\ 
are  the  only  possible  equilibria  of  the  shape  dynamics. 
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3.  For  a  given  value  of  A,  gift)  =  ge{t )  =  go(t)h\  is  a  relative  equilibrium 
for  the  system  (f-28)  under  the  left  action  of  group  SE( 2). 


4 ■  Letgxft)  be  the  Bertrand  mate  of  curve  g0(t)  indexed  by  X.  Then  gift)  = 
9e{t)  =  gxft). 


Proof 

We  have  shown  that  the  shape  dynamics  satisfies  equation  (4.29).  We 
know  that  £0  and  can  be  written  in  the  following  form 


fo 


0  —u0  1 

Mo  0  0 

0  0  0 


6 


0  —  uiUi  Vi 

viUi  0  0 

0  0  0 


(4.30) 


where  Uq  and  U\  are  steering  controls,  is  the  speed  for  vehicle  1.  If  there 
exists  he  s.t. 


+  hefift)  —  0  . 


(4.31) 


We  may  write  heft )  as 


heft) 


cos  9  —  sin  6  x 
sin  6  cos  9  y 

0  0  1 


(4.32) 


and  then  solve  for  9,  x  and  y.  Notice  that  because  he  is  an  equilibrium,  then 
9,  x  and  y  assumes  constant  values. 
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We  then  compute 


—Mo  sin  9  —  uo  cos  0  1  —  u^y 
uq  cos  6  —Mo  sin  9  uqx 
0  0  0 


(4.33) 


and 


he^l 


—i'iUism9  —  z/iMiCosd  z/i  cos 9 
z^iMi  cos  9  —uiuism9  v\  sin  9 
0  0  0 


Therefore,  the  following  set  of  equations  are  used  to  solve  for  he: 


(4.34) 


(mo  —  ^iMi)  sin#  =  0 

(mq  —  t'lMi)  cos  9  =  0  (4.35) 


ui  cos  9  +  u0y  —  1  =  0 
v\  sin  9  —  uqx  =  0  . 


(4.36) 


Obviously,  if  sin  0  =  0,  then  the  solutions  are  x  =  0,  y  =  A  and  =  1  —  Uq\. 
This  solution  is  h\.  We  have  proved  the  first  claim  that  he  =  h\  are  equilibria. 

To  prove  there  are  no  other  equilibria  if  Mo  is  not  constant,  we  assume 
that  sin  9  ^  0.  Then 


u0x  _  1  -  u0y 
sin  9  cos  9 


(4.37) 


which  implies 


uq(xcos9  +  y  sin  6)  =  1 


(4.38) 
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which  further  implies  that  uq  has  to  be  a  constant.  Therefore,  there  is  no 
solution  other  than  letting  sin  6  =  0. 

Let  ga  G  SE( 2)  denote  an  arbitrary  group  element  of  SE( 2).  Then  the 
left  action  of  ga  on  ( g±,go )  €  SE( 2)  x  SE(2)  is  dehned  as: 


®gabu9o]  =  (gagi,  gago)  ■ 


(4.39) 


The  lifted  action  on  (gi,go)  which  belongs  to  the  tangent  space  of  SE( 2)  x 
SE( 2)  is 

[01,00]  =  (gagiti,gago€o)-  (4.40) 

The  action  <f>9a  leaves  h  invariant  because 


®ga[hi\  =  ($5o[0o])  1  ( ^ Qa \.9 1 ] )  =  So  l9a  19a9i  =  So  19i  =  h  .  (4.41) 


The  lifted  action  T<bq  leaves  hi  invariant  because 


njh  i]  =  +  (t,.bo])-ld*a“lgl1 


dt  1  v  dt 

-idgo  _i  _i  -i  -i  c 

=  -9  0  ^So  9a  9a9 1  +  So  9a  SaSl£l 


—  — <Co^l  +  ^l£l 

=  h 


(4.42) 


Therefore,  equation  (4.29)  is  the  reduced  dynamics  for  equation  (4.28)  under 
the  left  action  of  symmetry  group  SE( 2). 

If  he(f)  =  h\  is  an  equilibrium  for  equation  (4.29),  then  from 


— £o  he  +  he£  i  —  0 


(4.43) 
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we  obtain  the  solution  for  £e  as 


0  — uo(t )  1  —  uoX 

&  =  u0(t )  0  0  (4.44) 

0  0  0 

Corresponding  to  he(t)  =  h\,  we  have  ge(t )  =  goh\.  Such  ge(t)  satisfies 

fje  9e£,e 

0  —  uo(t)  1  —  uqX 

=  9e  Uo(t)  0  0  •  (4.45) 

0  0  0 

We  notice  that  £e  =  £\.  From  the  definition  of  Bertrand  family,  we  see 
ge(t )  =  g\(t),  which  is  a  Bertrand  mate  for  go(t). 

To  show  that  ge(t)  and  go(t)  form  a  relative  equilibria,  we  shall  find  out 
r)  G  se( 2)  s.t.  the  infinitesimal  generator  of  such  rj  is  equal  to  the  vector  field 
at  ( ge,go )■  The  infinitesimal  generator  is 

Vc(ge,go)  =  (V9e,  V9o)-  (4-46) 

Then  we  shall  see  whether  we  can  solve  rj  from  the  equation 

(v9e,  V9o )  =  (ge£e,9o£i o)  (4.47) 

when  ge  =  gx  and  £e  =  £\.  Because  we  already  know 

£o  =  KteKx  ,  (4.48) 
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we  have 


(4.49) 


sofoSo 1  =  9ohe^e(goK)  1  =  ge^ege  1 

Thus  equation  (4.47)  does  have  a  unique  solution 

v  =  gotogo1  =  geiege1  •  (4.50) 

This  proves  that  (ge,  go)  is  a  relative  equilibrium  for  the  system  given  by 
equation  (4.28).  We  then  conclude  that  in  the  relative  equilibrium  state,  the 
trajectory  curves  traveled  by  the  two  particles  form  a  Bertrand  pair.  ■ 
We  noticed  that  the  vector  field  of  (gi,go)  is  non-autonomous  because  £o 
and  depend  explicitly  on  time.  Therefore,  a  Bertrand  pair  is  an  example 
of  a  relative  equilibrium  for  non-autonomous  systems. 

4.4  Controlled  motion  of  the  closest  point 

In  the  planar  setting,  consider  a  vehicle  moving  at  unit  speed  in  the  presence 
of  a  single  obstacle,  that  is,  the  region  enclosed  by  a  simple  closed  regular 
curve.  Suppose  that  at  each  instant  of  time,  the  point  on  the  obstacle  bound¬ 
ary  which  is  closest  to  the  moving  vehicle  is  unique.  This  point,  which  we 
will  call  the  “closest  point”  (or  “shadow  point”),  moves  along  the  boundary 
curve.  We  assume  uniqueness  of  the  closest  point  in  order  to  streamline  the 
discussion  and  bring  out  the  key  ideas.  Of  course,  in  dealing  with  real-world 
obstacles,  non-uniqueness  of  the  closest  point  is  an  important  issue. 

In  terms  of  the  arc-length  parametrization,  the  boundary  curve  can  be 
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described  by 


dr  i 
ds 
dx  i 
ds 
dy  i 
ds 


=  xi, 

=  yi«i, 

=  -X1/C1, 


(4.51) 


where  K]  is  the  plane  curvature  function  for  the  boundary  curve.  Using  the 
chain  rule,  we  can  express  the  time-evolution  of  the  closest  point  as 

ri  =  /yxi, 


where 


xi  =  yii^i/ci, 

yi  =  -X!i/i/ci, 

ds 

v^~t' 


(4.52) 


(4.53) 


with  s  denoting  the  arc-length  parameter.  Because  the  closest  point  depends 
on  the  motion  of  the  moving  vehicle,  V\  depends  on  both  the  boundary  curve 
and  on  the  trajectory  of  the  moving  vehicle. 

Letting  r2  denote  the  position  of  the  moving  vehicle,  x2  the  unit  tangent 
vector,  y2  the  unit  normal  vector,  and  w2  the  steering  control,  we  have  the 
following  system  of  equations  for  the  “formation”  consisting  of  the  moving 
vehicle  and  the  closest  point: 


rl  =  zbxi, 


Xl  =  yiZ'lKu, 


r2  =  x2, 


x2  =  y2w2, 


yi  =  -xiz/i/ci, 


y2  =  -x2u2, 


(4.54) 
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Figure  4.1:  Positions  and  frames  for  the  trajectory  of  the  moving  vehicle  (r2, 
x2,  and  y2)  and  for  the  closest  point  on  the  boundary  curve  (rx,  x1;  and  yly). 

where  kx  may  be  considered  given  (in  practice,  is  derived  from  sensor  data, 
e.g.,  from  a  laser  range  finder);  is  a  deterministic  function  of  (ri,xi,yi), 
(r2,x2,y2),  and  ny,  and  u2  is  the  control  input  we  apply  to  avoid  collid¬ 
ing  with  the  obstacle  and  to  achieve  boundary  following.  Our  objective  is 
to  determine  u2,  as  a  feedback  function  of  (ri,xi,yi),  (r2,x2,y2),  and  «i, 
such  that  we  can  prove  analytically  that  collision  avoidance  and  boundary 
following  are  achieved. 

Figure  4.1  illustrates  the  frames  for  the  trajectories  of  the  moving  vehicle 
and  the  closest  point  on  the  boundary  curve. 
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4.4.1  Boundary-curve  frame  convention 

We  define 

r  =  r2  — ri,  (4.55) 

to  be  the  vector  from  the  closest  point  on  the  boundary  curve  to  the  moving 
vehicle.  We  assume  that  initially 


r|  >  0, 


(4.56) 


and  we  will  prove  that  under  this  assumption  as  well  as  other  appropriate  hy¬ 
potheses,  our  boundary-following  control  law  guarantees  (4.56)  for  all  future 
time.  Then 


d_ 

dt 


r  •  r  r 

-77-  =  |-|  •  (x2  -  Z^Xi) 
|r|  |r| 


(4.57) 


The  first-order  necessary  conditions  for  the  closest  point  to  be  an  extremum 
of  the  Euclidean  distance  from  the  moving  vehicle  to  the  curve  are 


r  ■  xi  =  0, 


(4.58) 


and 

yi  =  ±A,  (4-59) 

|r| 

where  the  correct  choice  of  sign  depends  on  whether  the  boundary  curve  is 
to  the  right  or  left  of  the  moving  vehicle,  and  on  what  convention  is  chosen 
for  the  positive  direction  along  the  boundary  curve. 
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Using  (4.57)  with  (4.58)  and  (4.59),  we  obtain 


^|r|  =  ^j^j-  •  x2)  =  sgn(r  •  yi)yi  •  x2  =  -sgn(r  •  y^xi  •  y2  ,  (4.60) 

where  the  function  sgn(z)  is 


sgn(^) 

=  1 

for 

z  >  0 

sgn(z) 

=  0 

for 

z  =  0 

sgn(z)  = 

-1 

for 

z  <0  . 

(4.61) 

We  can  derive  an  expression  for  u\  by  differentiating  (4.58)  with  respect 
to  time,  to  obtain 

d  .  . 

—  (r  •  xU  =  r  •  xi  +  r  •  X! 

=  (x2  -  i/ixi)  •  X!  +  ^  ■  yi^  |r|i/i/ci 
=  Xi  •  x2  -  vx  +  sgn(r  ■  yi)|r|i/i/C]. 

=  0,  (4.62) 


where  we  have  used  equations  (4.58)  and  (4.59).  We  then  have 

(1  -  sgn(r  •  yx)  |r l^i)  vx  =  xx  •  x2,  (4.63) 


and  for  simplicity  we  replace  — sgn(r  •  yi)fiq  with  ±|ki|,  leading,  finally,  to 


xi  •  X2 

=  - — — - rr-r  >  0. 


(4.64) 


1  ±  | /ci  1 1  r  | 

In  (4.64),  the  plus  sign  is  used  when  the  boundary  curves  away  from  the 
moving  vehicle,  and  the  minus  sign  is  used  when  the  boundary  curves  inward 
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toward  the  moving  vehicle.  For  example,  if  the  boundary  curve  is  a  circle, 
then  we  use  the  plus  sign  when  the  vehicle  is  outside  the  circle  and  use  the 
minus  sign  when  the  vehicle  is  inside  the  circle.  In  the  latter  case,  we  note 
that  there  is  a  singularity  in  the  expression  for  v\  when  |r|  =  l/|fti|  and 
the  boundary  curves  inward  toward  the  moving  vehicle.  Later  we  will  see 
that  this  convention  for  determine  the  sign  for  curvature  agrees  with  the 
convention  we  use  in  section  4.2  for  a  Bertrand  family. 

4.4.2  Shape  variables 

We  would  like  to  treat  the  moving  vehicle  and  the  closest  point  on  the  bound¬ 
ary  as  two  interacting  “vehicles”  -  one  moving  at  unit  speed,  and  the  other 
not  necessarily  moving  at  unit  speed  -  and  then  demonstrate  analytically 
that  the  motion  of  the  two  “vehicles”  converges  to  a  steady-state  “forma¬ 
tion.”  This  approach  is  easiest  to  describe  if  appropriate  “shape  variables,” 
which  depend  only  on  the  relative  positions  and  orientations  of  the  vehicles, 
can  be  identified.  The  formation  is  then  an  equilibrium  for  the  shape  dynam¬ 
ics,  and  is  therefore  a  relative  equilibrium  for  the  dynamics  in  configuration 
space.  However,  in  order  to  reduce  the  dynamics  from  configuration  space 
to  shape  space,  the  control  law  must  possess  the  symmetry  group  SE( 2). 

In  the  proof  for  theorem  4.3.1,  we  showed  that  h\  is  invariant  under  the 
left  action  of  symmetry  group  SE{2).  Hence  as  components  for  h\ ,  the  shape 
variables  |r|  and  xi  -y2  are  invariant  under  the  left  action  of  symmetry  group 
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SE( 2).  Note  that  we  can  define  the  angle  variable  0  according  to 

xi  •  y2  =  sin  0,  (4.65) 

let  p  =  |  r  | ,  and  instead  use  (p,  0)  as  shape  variables. 

We  can  express  the  shape  dynamics  as  follows.  From  (4.60)  we  have 


P  =  — sgn(r  ■  yi)sin0, 


(4.66) 


where  sgn(r  •  yi)  is  constant  on  a  segment  of  smooth  curve. 
Differentiating  Xx  •  y2  with  respect  to  time  gives 

^  (xi  •  y2)  =  xi  •  y2  +  xi  •  y2 

=  ^i/ci(yi  •  Y2)  -  u2(x.i  ■  x2) 

Xi  •  X2 


1  ±  |/Ci||r 
K 1 


-  U2 


LV1  ±  l«il  p 

where  we  have  used  (4.64).  Since  we  also  have 


cos  0  —  u2 


(xi  •  x2) 
COS  0, 


^  (xi  •  y2)  =  ^ (sin 0)  =  (cos 0)0, 


we  obtain 


Kl 


COS  0  —  U2. 


(4.67) 


(4.68) 


(4.69) 


Thus,  provided  u2 
contained. 


1  ±  \m\ pt 

u2(p,  0),  we  see  that  the  shape  dynamics  are  self- 
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4.4.3  System  equations  on  SE( 2) 

The  motion  of  both  the  vehicle  and  the  closest  point  can  be  described  on 
SE( 2).  Let 


and 


9 1  = 


xi  yi  r! 
0  0  1 


9  2  = 


X2  y2  r2 
0  0  1 


Then  the  dynamics  on  SE( 2)  can  be  written  as 


dfh 

dt 

dg2 

dt 


9i€i 

fj2& 


(4.70) 


(4.71) 


(4.72) 


where 


and 


6 


0  —  V\ 

UlKi  0  0 

0  0  0 


6 


0  -u2  1 

w2  0  0 

0  0  0 


(4.73) 


(4.74) 


We  note  that  this  system  is  a  time-varying  system  because  v\  and  K\  depend 
on  time. 
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Next,  we  use  h  =  gx  1g2  to  represent  the  shape  of  the  formation  formed 
by  the  vehicle  and  the  closest  point.  Then 


Xi  ■  x2  Xi  ■  y2  X!  •  (r2  -  n) 
h=  yi  •  x2  yi  •  y2  yi  •  (r2  -  iq) 
0  0  1 

and  the  controlled  system  equation  for  the  shape  is 


(4.75) 


dh 

dt 


— £i  h  +  h^2  ■ 


(4.76) 


If  u2  is  designed  as  a  feedback  law  for  the  shape  variables,  then  system  (4.76) 
can  be  viewed  as  the  shape  dynamic  equations  for  system  (4.72). 


4.5  Tracking  of  boundary  curves 

We  want  to  understand  what  is  a  boundary  tracking  behavior  for  the  vehicle. 
We  say  a  vehicle  tracks  a  boundary  curve  with  separation  rc  if  the  trajectory 
of  the  vehicle  is  a  Bertrand  mate  of  the  boundary  curve  with  A  =  rc.  To  pro¬ 
duce  such  behavior,  we  propose  two  control  laws.  Under  these  control  laws, 
the  trajectory  of  the  vehicle  converges  to  a  Bertrand  mate  of  the  boundary 
curve  asymptotically. 

4.5.1  The  first  control  law  and  convergence  proof 

Let  f(p)  be  a  smooth  monotone  increasing  function  on  (0,  +oo)  such  that 

f(p)  =  0  when  p  =  rc 
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h 


2(1  +  kh 


<  f(p)  <  0  when  0  <  p  <  rc 


0  <f{p)  < 


h 


2(1  +  kki 


when  p  >  rc  . 


and 


f(p) 


k\ 


2(1  +  kki) 

rrc 

I  /  f(p)dp\  <  oo  , 


as  p  — >  +cx)  , 


where  k  >  0  and  h  >  0.  An  example  of  such  a  function  is 


f(p)  = 


k\ 


7r(l  +kki 


■  arctan(p  —  rc)  . 


We  define 


l  =  sgn(r  •  yi)  sin (0)  -  kf(p ) 


(4.77) 

(4.78) 

(4.79) 

(4.80) 

(4.81) 


where  k  >  0.  Then  by  using  the  shape  dynamics  (4.66)  and  (4.69),  and 
notice  that  sgn(r  •  y ] )  is  constant,  we  have  the  following 


i 


P 


df 

sgn(r  ■  yx)  cos (0)0  -  k-j-p 

dp 

/  \  r  ,\  /  \K1  COS  2(0)  .  ,.d/  .... 

— sgn(r  ■  yi)  cos (0)«2  +  sgn(r  •  yi)— — — ; - h  sgn(r  •  yi)k—  sm(0) 

1  ±  \Ki\p  dp 


~k  f(p)  —  l  . 


(4.82) 


Proposition  4.5.1  Given  the  control  law 

«2  =  — 1-7~-  ( 717  y  +fe^sin(0)  -sgn(r -yi)(/(p)  -  hi)]  (4.83) 

cos(0)  \  i  ±  |«i |p  dp  j 

where  the  choice  of  +  or  —  sign  is  determined  by  the  shape  of  the  curve  and 
the  position  of  the  robot.  Suppose  all  the  curve  segments  which  result  in  the 
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choice  of  —  sign  have  curvature  bounded  by  a  constant  kc  i.e.  |/ci|  <  kc. 
Furthermore,  let 

VM)=  [  f(z)dz  +  tl2.  (4.84) 

If  the  initial  condition  for  the  robot  is  such  that  |cos(0(to))|  ^  0  and 

V(p(t0),<i>(to))  <  min{0,  /  f(z)dz}  ,  (4.85) 

Jo 

then  U2  produces  converging  shape  dynamics  s.t.  sin (0(f))  — >  0  and  pit )  — >  rc 
as  t  — >  oo.  In  addition,  the  robot  will  not  collide  with  the  boundary  curve 
segment  where  the  closest  point  belongs. 

Proof 

We  first  prove  that  if  the  initial  condition  is  such  that  2nn  <  4>{t0)  < 
2nn  +  7t/2  where  n  is  an  integer,  then  fit)  <  2rm  +  ti/2  for  all  t  >  t0.  In 
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Therefore,  it  is  clear  that  0(f)  <  0  if  0(f)  G  (2wr  +  7t/2  —  e,  2nn  +  n/2).  We 
deduce  that  0(f)  <  2mr  +  7t/2  for  all  f  >  f0  given  2n7r  <  0(fo)  <  2nii  +  ir/2. 

Next,  we  prove  that  if  the  initial  condition  is  such  that  2nn  —  n /2  < 
0(fo)  <  2nn,  then  0(f)  >  2nn  —  7t/2  for  all  f  >  f0. 

In  fact,  under  the  proposed  control  law,  one  has 


> 


COs(0) 

X  7  11 

l±|«l|p 

1  I 

(-< 

COs(0) 

1  I 

(-< 

COs(0) 

_L  U.  ^ 


hu 


Because  df  /dp  >  0,  if  0(f)  G  (2nir  —  7t/2,  2n7r),  then 


(k-f-  +  fci)  sin(0)  <  0  .  (4.90) 

dp 

Furthermore,  for  a  given  p,  there  exists  e'(p,5)  >  0  such  that  if  0(f)  G 
(2n7r  —  7t/2,  2wt  —  ir/2  +  e'),  then 

~^k~dp  +  ^1)sin(^)  >  y  •  (4-91) 

Therefore,  it  is  clear  that  0(f)  >  0  if  0(f)  G  ( 2nir  —  7t/2,  2n7r  —  ir/2  +  e').  We 
deduce  that  0(f)  >  2mr  —  tt/2  for  all  f  >  f0  given  2n7r  —  7t/2  <  0(fo)  <  2-U7T. 

By  similar  argument,  we  can  show  that  0(f)  G  (2n7T  +  n/2,  2m:  +  37t/2) 
for  all  f  >  f0  given  2n7T  +  7t/2  <  0(fo)  <  2n7r  +  37t/2.  Therefore,  our  control 
law  is  well  defined  for  all  f  >  t0l  if  initially  cos(0(fo))  ^  0.  This  statement 
also  confirms  that  sgn(r  ■  yi)  is  a  constant  under  the  controlled  dynamics. 
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Let  a  candidate  Lyapunov  function  be  V  —  f( Qp  f(z)dz  +  |/2.This  function 
is  bounded  below  by  fjc  f(z)dz.  Then 


V  =  ) 

K/ cos  ^  i(j)\  df 

+  /(— sgn(r  •  yi)  cos {4>)u2  +  sgn(r  ■  yi)  '  ;  +  sgn(r  •  yi )k-j-  sin(0)) 

1  ±  \K\p  dp 


=  f(p)(-kf(p)-l)+l(f(p)-k1l) 
=  -kf2(p)  -  hi2  <  0 


(4.92) 


with  V  —  0  iff  p  —  rc  and  l  =  0.  Therefore,  by  Lyapunov  stability  theory, 
l  — >  0  and  p  — >  rc  as  t  — >  oo.  This  further  implies  that  sin ((f))  — »  0. 

If  during  the  convergent  process  p  =  0,  then  the  robot  collides  with  the 
obstacle.  Notice  that  when  p  —  0,  the  minimum  value  for  the  Lyapunov 
function  V  is  0.  Therefore,  if  the  initial  condition  for  the  robot  satisfies 


V{p{to),<j>(t0))  <  0  ,  (4.93) 

then  because  along  the  controlled  robot  dynamics  V  is  decreasing,  the  robot 

will  not  assumes  the  state  where  p  =  0.  Thus  the  whole  path  is  collision  free. 

1 

On  the  other  hand,  the  condition  V ( p(to ),  <j>(to))  <  JcTc  f(z)dz  guarantees 
that  on  the  trajectory  of  the  robot,  if  the  —  sign  is  chosen  in  -u2,  then  the 
control  u2  does  not  go  to  infinity.  ■ 

The  limiting  case  sin(0)  =  0  has  two  possible  solutions:  (p  =  2mr  or 
(p  =  2wr  +  tv.  Which  solution  is  achieved  depends  on  the  initial  value  </>(£0), 
which  is  determined  by  the  choice  of  positive  direction  of  the  boundary  curve. 
If  the  positive  direction  of  the  curve  is  chosen  so  that  xi(£0)  -x2(to)  >  0,  then 
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4>(to)  €  ( 2mr  —  tt/2,  2mr  +  n/2)  so  that  0  — >  2nn.  The  vehicle  moves  to  the 
same  direction  as  the  positive  direction  of  the  curve.  If  x^to)  •  x2(t0)  <  0, 
then  4>{t0)  e  (2wr  +  7r/2,2mr  +  37t/2)  so  that  (f)  — >  2wr  +  ir.  The  vehicle 
moves  against  the  positive  direction  of  the  curve.  Comparing  the  two  cases, 
we  conclude  that  the  direction  of  the  limiting  vehicle  motion  is  independent 
of  the  choice  of  the  positive  direction  of  the  boundary  curve.  Therefore,  for 
simplicity,  we  may  assign  positive  direction  to  a  boundary  curve  by  always 
requiring  x^o)  •  x2(t o)  >  0.  Therefore  the  limiting  motion  of  the  vehicle 
always  achieves  (f)  — »  2wr. 


4.5.2  Stabilized  relative  equilibrium 


As  t  — >  oo,  because  <f>{t)  — >•  2mr,  we  have 


h0  =  lim  h(t)  =  lim  g1  g2  = 

t — >oo  t— >  OO 


and 


Then 


Zi(t) 


i  o  o 

0  1  ±rc 
0  0  1 


u2(t) 


K 1 


1  ±  \Ki\rc 


1 


1  ±  /Cl  rc 


K1 


1 


l±|/ti|rc  l±|/ti|rc 


gi 


l±|Ki|rc 


0 


0 

0 


0 

0 


(4.94) 


(4.95) 


(4.96) 


(4.97) 
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and 


0  -Ufe  1 

&(*)“>  life  0  0  •  (4‘98) 

0  0  0 

The  limiting  vehicle  trajectory  is  a  Bertrand  mate  to  the  boundary  curve. 
To  see  this,  we  denote  the  limiting  trajectory  as  (goo(t),  ^(t))  and  its  cor¬ 
responding  Lie  algebra  as  ^  i.e.  £2(£)  - >  £<x>(t)-  This  curve  satisfies  the 
following  differential  equation  on  SE( 2) 


Soo  l/ooSoo 

0  ^OO  f 

=  goo  Koo  0  0  •  (4.99) 

0  0  0 

We  use  this  limiting  trajectory  as  the  reference  curve.  Comparing  equation 
(4.98)  with  equation  (4.99),  it  is  clear  that 


1  ±  |«q|r c 


^OO 


(4.100) 


hence 


1  +  sgn(r  •  yx) 

^OO^C 

Then  if  we  use  t  as  parameter,  the  boundary  curve  Ki(t))  satisfies 


(4.101) 


0  fc'OG  1  +  sgn(r  •  yi) 

^OO^C 

9i  =  9i  0  0  •  (4.102) 

0  0  0 
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We  can  rewrite 


1  +  sgn(r  •  yi)/c0 crc  =  1  -  k^X  (4.103) 

if  we  let  A  =  — sgn(r  •  yi)rc.  Comparing  to  the  definition  for  Bertrand  mate 
in  section  4.2,  we  immediately  see  that  in  the  steady  state,  gi(t)  and  goo(t) 
form  a  Bertrand  pair  with  g^t)  serving  as  the  reference  curve. 

Proposition  4.5.1  tells  us  that  the  relative  equilibrium  h(t)  =  h,0  is  as¬ 
ymptotically  stable  for  initial  values  h( 0)  €  SE( 2)  except  for  the  ones  with 
cos(0(O))  =  0.  Starting  with  h(0),  the  trajectory  of  the  vehicle  converges 
to  a  Bertrand  mate  of  the  boundary  curve.  On  the  other  hand,  the  initial 
condition  cos(0(O))  =  0  is  unstable.  Therefore,  due  to  existence  of  noise,  the 
vehicle  will  not  adhere  to  this  condition. 

4.5.3  Tracking  piece-wise  smooth  curves 

In  reality,  instead  of  being  regular,  almost  all  boundary  curves  are  only  piece- 
wise  regular.  For  example,  obstacles  with  polygonal  shape  appear  every¬ 
where.  We  say  a  point  on  the  boundary  is  a  vertex  if  the  tangent  vector  at 
this  point  is  not  well-defined.  Suppose  a  vertex  appears  when  s  =  so-  Then  if 
a  tangent  vector  on  each  side  of  the  vertex  is  well  defined,  i.e.  lims_^s+  dr/ds 
and  lims^s-  dr/ds  exists,  we  say  the  vertex  is  of  type  one.  At  a  type  one 
vertex,  there  is  a  sudden  change  in  the  direction  of  the  curve. 

There  are  possibilities  that  the  closest  point  may  be  a  vertex.  If  this 
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Figure  4.2:  A  crack  with  a  type  one  vertex.  Under  the  first  control  law,  the 
vehicle  will  crash  with  the  vertex. 

happens,  we  assign  a  unit  tangent  vector  X!  to  the  vertex  such  that 

r  ■  xx  =  0  (4.104) 

is  still  enforced.  We  may  also  let  K\  —  0  at  the  vertex.  As  long  as  there 
are  only  finitely  many  vertices,  and  the  curve  segments  between  vertices 
are  sufficient  long,  then  the  vehicle  may  use  our  first  control  law  to  track 
the  regular  boundary  curve  segments.  Near  the  vertices,  the  vehicle  will 
be  disturbed  by  the  discontinuity  in  the  direction  of  tangent  vector  and  in 
curvature.  It  will  deviate  from  the  Bertrand  mate  of  the  regular  boundary 
curve  segments.  However,  since  the  discontinuity  only  happens  at  the  vertex, 
the  vehicle  will  come  back  to  follow  the  Bertrand  mate  under  the  first  control 
law. 

A  more  severe  problem  caused  by  vertices  is  the  possible  existence  of  a 
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Figure  4.3:  A  crack  with  new  convention  for  the  direction  of  the  curve.  Under 
the  second  control  law,  the  vehicle  will  track  the  boundary  in  given  direction. 

crack ,  that  is,  two  segments  of  boundary  near  a  type  one  vertex  shown  in 
Figure  4.2.  When  the  direction  of  the  boundary  curve  is  chosen  as  shown, 
the  angle  6  is  less  than  or  equal  to  7t/2.  If  a  vehicle  moves  in  the  crack  i.e. 
the  space  between  the  two  curve  segments  near  the  vertex,  then  under  our 
first  control  law,  the  vehicle  will  be  guided  toward  the  vertex  and  eventually 
collide  with  the  obstacle,  as  shown  in  Figure  4.2. 

To  solve  this  problem,  a  new  convention  is  needed.  When  the  vehicle 
detects  a  crack,  we  assign  the  clock-wise  or  counter  clock-wise  direction  as 
the  positive  direction  for  all  the  detected  curve  segments  that  form  the  crack. 
Therefore,  the  closest  point  detected  by  the  vehicle  in  the  crack  may  result  in 
f  G  (— 7r,  +7r),  instead  of  (— 7t/2,7t/2)  restricted  by  the  previous  convention. 
This  new  convention  is  shown  in  Figure  4.3. 

We  also  need  a  new  boundary  tracking  control  law,  which  enables  the 
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vehicle  to  track  boundary  curves  in  an  assigned  positive  direction.  Such 
a  control  law  would  complement  the  first  control  law  which  perform  the 
tracking  in  a  direction  that  is  independent  of  the  direction  of  a  boundary 
curve.  We  follow  the  similar  process  in  deriving  the  first  control  law  by 
starting  from  the  shape  dynamics 


P  =  — sgn(r  •  yi)sin(0) 

k  1 


COS  (j)  —  U2. 


(4.105) 


1  ±  |/ci|p/ 

Let  f(p )  be  a  smooth  monotone  increasing  function  on  (0,  +oo),  with  f(p)  = 
0  if  and  only  if  p  —  rc  .  Define 


l  =  sgn(r  •  yi)2  sin(0/2)  -  kf(p )  . 


(4.106) 


Then 


P  =  -cos(|)/c/(p)  -  cos(|)/ 

j  f  s  ,<t>,  ,  ,  Kicos(0)cos(f) 

/  =  — sgn(r  •y1)cos(-)'u2  +  sgn(r  •  yj — — — ; - 

2  1  ±  \Ki\p 

+sgn(r  ■  yi)k-/~  sin(0). 
dp 

The  control  law  u2  is  designed  as 
kic°s(0)  df  . 


u2  = 


1  ±  |/ci|p  dp 


+  2k—  sin(0/2)  -  sgn(r  •  yx)  f(p)  -  h l 


so  that 


/  =  cos(|)  (f(p)  -  hi) 


(4.107) 


(4.108) 


(4.109) 


where  k\  >  0. 
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We  now  prove  the  convergence  of  the  controlled  dynamics.  The  Lyapunov 
candidate  function  is 

V=H(p)+l-l2.  (4.110) 

where  H(-)  is  continuously  differentiable  and  satisfies  the  following  hypothe¬ 
ses. 

(Al)  dh/dp  =  f(p),  where  f(p)  is  a  smooth  monotone  increasing  function 
on  (0,  ex)),  so  that  h(p)  is  continuously  differentiable  on  (0,  oo); 

(A2)  7(rc)  =  0  and  f(p )  ^  0  if  p  ^  rc; 

(A3)  |>™|7(/»)|-^; 

(A4)  lim^o  h(p)  =  oo,  liiiip^^  H(p)  =  oo,  and  3 p  such  that  H(p)  =  0. 

With  control  «2  given,  we  now  compute  its  derivative  as 

v  =  f(p)p  +  ii 

=  f(p)(-  cos (|)fc  f(p)  -  cos(^)  /)  +  Z(cos(|)/(p)  -  COS (^fal) 

=  -  cos(^)  kf2(p)  -  cos(^)  hi2  <  0  .  (4.111) 

If  0(f)  G  (— 7T, +7r),  then  V  —  0  if  and  only  if  f(p)  =  0  and  l  =  0.  While 
f{p)  —  0  implies  that  p  —  rc,  l  —  0  is  used  to  conclude  sin(|)  =  0,  which 
gives  one  unique  answer  0  =  0.  Therefore,  by  Lyapunov  stability  theorem, 
the  system  dynamics  under  control  law  U2  converges  asymptotically  to  the 
equilibrium  p  =  rc  and  0  =  0. 


139 


When  p  — >  0,  the  function  V  — >  oo.  Then  if  the  robot  start  with  a  state 
p  7^  0,  the  value  of  V"  is  finite.  Along  the  controlled  dynamics  V  will  decrease. 
Therefore,  the  robot  will  not  collide  with  any  obstacle. 

We  notice  that  the  configuration  when  <p  =  ±7r  is  unstable.  Because  of 
condition  (A3),  by  similar  analysis  for  the  first  control  law,  we  know  that 
slight  perturbation  of  the  angle  <p  will  cause  the  vehicle  converging  to  (p  =  0. 
In  reality,  since  <p  is  measured  by  noisy  sensors,  we  almost  never  can  get 
cp  =  ±7r  exactly.  Therefore,  we  consider  this  as  a  problem  that  can  be 
omitted.  In  summary,  we  have  proved  the  following  proposition. 


Proposition  4.5.2  Given  control  law  u2  as  in  equation  (4-108).  Suppose  all 
the  curve  segments  which  result  in  the  choice  of  —  sign  in  U2  have  curvature 
bounded  by  a  constant  nc  i.e.  \ki\  <  nc.  If  the  initial  condition  < p(t0 )  G 
(—7 r,  7 r)  and 

V(p(to)A(to))  <  ti(— )  ,  (4.112) 

Kc 

then  the  control  law  given  by  (4-108)  produces  converging  shape  dynamics  s.t. 
(pit)  — >  0  and  pit )  — > ►  rc  as  t  — »  oo.  Furthermore,  the  robot  will  not  collide 
with  the  boundary  curve  where  the  closest  point  belongs. 

We  point  out  that  there  exists  functions  f(p)  and  h(p)  which  satisfy  the 
conditions  (Al)-(A4).  We  let 


f(p) 


h  (i_i£ 

1  +  ki  k v  p 


(4.113) 


This  function  is  clearly  monotone  increasing  with  f(p)  =  0  if  and  only  if 
p  =  rc.  We  then  need  to  verify  condition  (A3).  First,  we  noticed  that  if 
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p  >  tc,  then  because 


it  is  true  that 


0  <  f(p)  < 


1  +  kk\ 


^>o>i±M/W-h. 

dp  k  k 


Next,  when  p  <  rc,  we  want  to  show 


(4.114) 


(4.115) 


h  r c  _  [c,  _  fa 

1  +  fa  k  p2  k  p  k  ’ 


(4.116) 


which  is  equivalent  to  show 


2p  “  r‘p  +  T7kTkr‘ >  0 


(4.117) 


Suppose  we  select  the  value  of  k  and  k\  so  that 


1  +  ki  k 


(4.118) 


Then  the  inequality  (4.117)  is  satisfied.  Therefore,  the  condition  (A3)  is 
satisfied.  Then  an  h(p)  that  satisfies  condition  (A4)  is 


Kp)  =  — j ~  k  ( P  ~  rc  log (p)  -  r c  +  rc  log(rc))  (4.119) 


with  H(rc)  =  0. 


4.6  Simulation  results  of  boundary  tracking 


In  this  section  we  present  simulation  results  verifying  the  two  boundary  track¬ 
ing  control  laws.  Our  simulated  robot  is  equipped  with  32  range  sensors 
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evenly  distributed  across  the  360  degree  perimeter  of  the  robot.  Since  the 
angle  between  two  sensor  rays  are  not  arbitrarily  small,  the  movement  of  the 
closest  point  could  be  discontinuous.  We  will  see  this  effect  in  the  results. 

To  test  the  first  control  law,  we  use  an  elliptic  obstacle  with  smooth 
boundary  curve.  The  trajectories  are  shown  in  figure  4.6,  the  initial  condition 
of  the  robot  determines  the  direction  of  the  trajectory  circling  the  obstacle. 


Figure  4.4:  Trajectories  of  boundary  tracking  of  an  elliptic  obstacle. 

The  shape  variables  (j)  and  p  are  plotted  in  figure  4.5(a)  and  4.5(b).  One 
can  see  the  averaged  value  for  the  angle  <fi  converges  to  0.  The  averaged  value 
for  p  converges  to  0.95  which  is  very  close  to  1,  the  designated  separation.  The 
curvature  value  at  the  closest  point  is  plotted  in  4.5(c).  We  see  oscillations 
in  all  these  data.  This  is  because  there  are  limited  number  of  sensor  rays, 
therefore  the  detected  closest  point  could  be  jumping  from  one  ray  to  another, 
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causing  discontinuities. 

We  then  use  a  nonconvex  polygon  as  the  obstacle  to  test  the  first  tracking 
control  law.  The  trajectories  are  shown  in  figure  4.6.  Again,  the  direction  of 
the  trajectories  are  determined  by  the  initial  orientation  of  the  robot. 

The  shape  variables  (p  and  p  corresponding  to  figure  4.6(a)  are  plotted  in 
figure  4.7(a)  and  4.7(b).  We  see  clearly  that  vertices  cause  large  disturbances 
in  cp  and  p.  Here  the  designated  value  for  p  is  0.6.  One  can  see  that  after  each 
vertex  the  control  law  tries  to  control  <p  and  p  back  to  the  desired  value.  But 
from  the  data  of  (p  and  p  it  is  difficult  to  recognize  when  a  vertex  is  detected. 

The  curvature  value  at  the  closest  point  is  plotted  in  figure  4.7(c).  We 
discovered  that  the  upward  and  downward  spikes  in  this  curvature  plot  has 
clearly  indicated  where  the  vertices  are.  The  eight  spikes  in  the  curvature 
plot  correspond  to  the  eight  vertices  of  the  obstacle. 

Under  the  second  control  law,  the  robot  always  circling  the  obstacle  in  the 
same  direction.  For  the  elliptic  obstacle,  the  trajectory  is  plotted  in  figure 
4.6. 

The  shape  variables  (p  and  p  for  figure  4.8(a)  are  plotted  in  figure  4.9(a) 
and  4.9(b).  The  curvature  value  at  the  closest  point  is  plotted  in  figure  4.9(c). 
The  plots  are  quite  similar  to  those  of  the  first  tracking  control  law. 

We  then  used  a  polygon  with  a  crack  to  test  the  boundary  tracking  ability 
of  the  second  control  law.  The  trajectory  is  shown  in  figure  4.6.  We  see  that 
the  tracking  is  successful. 

The  shape  variables  <p  and  p  are  plotted  in  4.10(a)  and  4.10(b).  Again 
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(a)  <f>(t) 


(b)  p(t) 


(c)  Ki  (t) 

Figure  4.5:  Shape  variables  and  curvature  measured  at  the  closest  point  when 
tracking  an  elliptic  obstacle. 
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Figure  4.6:  Trajectories  of  boundary  tracking  of  a  nonconvex  polygon. 

we  see  big  disturbances  caused  by  the  vertices.  The  curvature  value  at  the 
closest  point  is  plotted  in  4.10(c).  The  spikes  in  the  curvature  measurements 
correspond  to  the  vertices  with  one  exception.  The  vertex  that  serves  as  the 
peak  of  the  crack  is  never  a  closest  point  to  the  robot.  Therefore,  there  is  no 
corresponding  spike  in  the  curvature  plot. 
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(a)  <f>(t) 


(b)  p(t) 


(c)  m  (t) 

Figure  4.7:  Shape  variables  and  curvature  measured  at  the  closest  point  when 
tracking  a  nonconvex  polygon. 
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(a)  0(0)  <  0 

Figure  4.8:  Trajectories  of  boundary 
the  second  control  law. 


(b)  0(0)  >  0 

tracking  of  an  elliptic  obstacle,  using 


4.7  Navigation  using  boundary  tracking  be¬ 
haviors 


We  consider  a  plane  with  finitely  many  static  obstacles,  each  one  is  mod¬ 
eled  as  a  simply  connected  compact  set  with  a  smooth  boundary  curve  with 
bounded  curvature.  A  moving  vehicle  on  this  plane  has  the  ability  to  detect 
a  fixed  point  target  located  at  the  origin  of  a  right  handed  coordinate  system 
with  the  direction  of  the  X-axis  arbitrarily  chosen.  Furthermore,  we  assume 
that  for  each  obstacle,  there  exists  a  cone  with  its  apex  located  at  the  ori¬ 
gin  such  that  the  obstacle  is  contained  in  the  cone.  Notice  that  the  cones 
mentioned  here  all  have  open  angles  less  than  7 r. 
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1.6 


(a)  <f>(t) 


(b)  p(t) 


(c)  Ki  (t) 

Figure  4.9:  Shape  variables  and  curvature  measured  at  the  closest  point  when 
tracking  an  elliptic  obstacle,  using  the  second  control  law. 
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Figure  4.10:  Shape  variables  and  curvature  measured  at  the  closest  point  when 
tracking  a  nonconvex  polygon,  using  the  second  control  law. 
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Figure  4.11:  Trajectories  of  boundary  tracking  of  a  nonconvex  polygon,  using 
the  second  control  law. 

The  vehicle  is  able  to  detect  segments  of  the  boundary  curves  of  obstacles 
by  using  sensor  rays  and  is  able  to  estimate  the  tangent  vector  and  curvature 
of  the  boundary  curve  at  any  detected  point.  As  an  idealization,  we  suppose 
that  each  sensor  ray  has  infinite  range  of  detection.  Furthermore,  the  vehicle 
has  enough  sensor  rays  that  are  able  to  scan  a  full  360  degree  view.  Our 
goal  is  to  design  a  control  law  which  enables  the  vehicle  to  get  within  a  small 
neighborhood  of  the  target  without  colliding  with  any  of  the  obstacles. 

4.7.1  Virtual  boundary  segments 

For  each  obstacle  denoted  by  Obj  where  (j  =  1,2,3,  ...,1V),  we  define  a  set 
Hj  to  be  the  set  of  all  line  segments  connecting  points  of  the  set  Obj  and  the 
target  o. 

Hj  =  {q(x,  y) |q  —  to+(l  —  t) b,  t  G  [0, 1],  b  G  Obj}  (4.120) 
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For  convenience  of  discussion,  we  denote  the  boundary  curve  of  Obj  by  Fj, 
denote  the  boundary  curve  of  Hj  by  A j,  as  shown  in  Figure  4.12.  We  want 
to  know  the  structure  of  A j. 

From  figure  4.12,  it  is  intuitive  that  A j  contains  two  straight  line  segments 
ow!  and  ow2  and  part  of  F, .  We  shall  rigorously  prove  this. 


Figure  4.12:  The  construction  of  virtual  boundary  curve  segments. 

Claim  4.7.1  Hj  can  be  constructed  as  the  collection  of  all  line  segments  that 
connect  points  of  Fj  and  o 

Proof 

Let 

Hj  =  {qi(x,j/)|qi  =  tto+  (1  —  ti)bx,  ti  G  [0,1],  bx  G  Tj}  (4.121) 

For  any  qi  G  Hj,  because  b |  G  Fj  C  Obj,  we  know  qi  G  Hj.  Hence  Hj  C  Hr 
On  the  other  hand,  for  any  q  G  Hj,  if  b  G  hit (Obf),  one  can  extend  the  ray  ob 
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so  that  this  ray  intersects  with  Tj  at  point  bi.  One  can  do  this  because  Obj 
is  compact.  Then  a  reparametrization  shows  q  G  Hj.  Therefore  Hj  C  Hj. 


Claim  4.7.2  The  boundary  curve  of  Hj  passes  through  o  i.e.  o  G  A j. 

Proof 

Use  proof  by  contradiction.  We  assume  that  o  is  in  the  interior  of  Hj  i.e. 
o  G  in t(Hj).  Then  consider  a  ray  emit  from  o.  The  ray  intersects  Tj  at  some 
point  bi.  When  the  ray  sweeps  360  degree  the  intersection  point  traces  a 
closed  curve  that  encircles  o.  This  contradicts  with  the  assumption  that  Obj 
is  contained  in  a  cone  with  its  peak  point  at  o.  ■ 

Claim  4.7.3  The  boundary  curve  of  Hj  contains  two  segments  of  straight 
lines  owi  and  ow2  where  wy,  w2  G  T  r  The  extensions  of  the  straight  line 
segments  beyond  wy  and  w2  have  no  more  intersection  with  Obj. 

Proof 

Because  Obj  is  compact,  each  straight  line  segment  that  connects  o  with  a 
point  on  Tj  must  has  a  intersection  bi  with  Tj  such  that  the  distance  between 
o  and  bi  is  the  maximum  among  all  the  intersections  between  Obj  and  the 
straight  line.  If  the  line  segment  obi  has  another  intersection  b2  with  Tj  such 
that  the  line  segment  bib2  G  Obj,  then  the  boundary  curve  Tj  is  partitioned 
into  two  parts  with  each  part  lying  on  one  side  of  the  line  bib2,  as  shown  in 
Figure  4.12.  Hence  at  the  point  b2,  we  can  find  a  small  boundary  segment 
with  its  points  on  both  sides  of  bib2.  Connecting  the  two  end  points  of  such 
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segments,  Vi  and  V2  with  o,  we  can  see  that  ovi  e  Hj  and  0V2  G  Hj.  We 
observe  that  there  exists  a  small  enough  neighborhood  of  b2  with  radius  less 
than  ||  b2V!  ||  and  ||  b2v2  ||  that  is  entirely  contained  in  Hj.  Therefore,  b2  is 
not  on  the  boundary  of  Hj.  This  implies  that  ob2  is  not  part  of  the  boundary 
of  Hj.  Thus  for  obx  to  be  part  of  the  boundary  curve  of  Hj ,  bx  and  all  the 
intersections  between  obx  and  Obj  must  be  tangent  points.  Therefore,  obi 
must  be  a  tangent  line  of  the  boundary  of  Obj. 

Because  Obj  can  be  contained  in  a  cone  with  its  apex  at  o,  there  exists 
tangent  lines  like  obi.  We  can  only  have  two  such  tangent  lines  because  if 
there  is  a  third  one,  one  line  must  lie  between  the  other  two,  hence  fails  to 
be  a  segment  of  the  boundary  curve.  Therefore,  we  can  find  points  wy  and 
w2  on  Y j  where  owi  and  ow2  are  tangent  to  the  boundary  curve  Tj  so  that 
owi  and  0W2  are  boundary  segments  of  Hj.  We  name  the  tangent  points 
with  maximum  distance  to  o  along  the  lines  as  W!  and  w2.  ■ 

Definition  4.7.4  We  define  the  two  straight  line  segments  ow!  and  ow2  as 

virtual  boundary  segments  of  the  set  Hj. 

Claim  4.7.5  For  any  point  w  e  TJ;  Obj  lies  entirely  on  one  side  of  a  line 
obtained  by  extending  the  segment  ow,  if  and  only  if  ow  C  owj  U  ow2. 

Proof 

If  ow  C  ow!  U  ow2,  without  loss  of  generality,  suppose  ow  is  aligned  with 
owx.  Then  by  definition,  all  points  in  this  line  that  belongs  to  Obj  are  tangent 
points.  Now,  suppose  there  are  points  belong  to  Obj  on  both  sides  of  the 
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extended  line  owi .  Because  Obj  is  simply  connected,  there  exists  a  path  that 
connect  these  two  points  and  lies  entirely  within  int (Obj).  Such  path  must 
intersect  owi  and  the  intersection  point  can  not  be  a  tangent  point  because 
it  is  not  a  boundary  point  of  Obj.  Therefore,  by  contradiction,  we  proved 
that  Obj  is  located  on  one  side  of  the  extended  line. 

If  we  know  Obj  is  located  on  one  side  of  the  extended  line  ow,  w  must 
be  a  tangent  point  and  all  possible  intersections  between  the  extended  line 
and  Obj  must  be  tangent  points,  because  otherwise  there  will  be  boundary 
points  of  Obj  on  both  sides  of  the  line.  Therefore,  ow  is  a  virtual  segment 
that  passes  through  o.  As  mentioned  before,  the  only  two  possible  lines  are 
ow!  and  ow2.  Hence  ow  C  owj  U  ow2.  ■ 

4.7.2  Detection  of  virtual  boundary 

We  already  developed  a  boundary  following  control  law.  Is  it  possible  to  use 
such  a  control  law  to  allow  the  vehicle  to  move  towards  the  target?  One 
answer  may  be  to  enable  the  robot  to  follow  the  virtual  boundary  segments 
instead  of  the  true  boundary  curve  in  certain  context,  taking  advantage  of 
the  two  virtual  segments  which  pass  through  the  target.  Since  the  virtual 
boundary  segments  do  not  exist  physically,  we  must  provide  an  algorithm 
for  the  moving  vehicle  to  construct  the  virtual  segments  based  on  its  sensor 
readings.  A  simple  algorithm  exists:  to  construct  one  virtual  boundary  seg¬ 
ment,  the  vehicle  only  need  to  detect  the  tangent  point  wj  or  w2  on  the  true 
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boundary  curve  Tj.  Because  the  vehicle  knows  where  the  target  is,  knowing 
a  tangent  point  is  enough  to  determine  a  virtual  segment. 

We  assume  that  the  sensor  rays  of  the  moving  vehicle  have  infinite  de¬ 
tection  range.  Then,  if  there  is  only  one  obstacle  Ob±,  one  possibility  for 
the  vehicle  to  fail  to  detect  wy  is  when  wy  is  invisible  to  r,  that  is,  the  line 
connecting  the  vehicle  r  and  Wj  intersect  Ti  at  some  point  other  than  wy. 
We  can  apply  the  same  argument  to  w2.  The  two  cases  when  both  wy  and 
w2  are  invisible  are  shown  in  Figure  4.13  and  4.14. 


Figure  4.13:  The  vehicle  lies  inside  the  cone  formed  by  owj  and  ow2.  Both 
wi  and  w2  are  invisible  by  the  moving  vehicle. 

Claim  4.7.6  Either  w |  or  w2  is  visible  to  r,  if  r  lies  outside  the  cone  formed 
by  rays  owi  and  ow2  that  contains  Obi . 

Proof 
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Figure  4.14:  Another  case  where  the  vehicle  lies  inside  the  cone  formed  by 
owi  and  0W2,  both  wj  and  W2  are  invisible.  However,  the  vehicle  is  able  to 
detect  a  segment  of  F  ]  which  is  also  a  segment  of  A ! . 


Figure  4.15:  When  the  vehicle  lies  outside  the  cone,  either  wj  or  W2  can  be 
detected. 
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The  obstacle  Obi  lies  between  the  rays  owi  and  ow2.  If  r  lies  outside  this 
cone,  then  r  ^  Hi. 

We  now  prove  by  contradiction.  Suppose  neither  w4  nor  w2  are  visible 
by  the  vehicle  at  r.  Then  the  line  segments  rwi  and  rw2  must  intersect  the 
boundary  curve  of  Obi  at  points  v4  and  v2.  We  know  v4  e  Hi  and  v2  6  Hi. 

Because  r  is  outside  the  cone  that  contains  the  obstacle,  we  know  that 
both  line  segments  rvi  and  rv2  must  intersect  with  the  cone  boundary  which 
consists  of  two  extended  rays,  ow4  and  ow2.  There  are  three  possible  con¬ 
figurations. 

First,  suppose  both  rvi  and  rv2  intersect  with  ow2  or  its  extension,  as 
shown  in  Figure  4.16.  Denote  the  intersection  points  as  v3  and  v4.  Then 
we  observe  that  there  are  two  distinct  intersections,  v4  and  w2,  between  two 
straight  lines  ow2  and  rw2.  Therefore,  this  is  impossible. 


Figure  4.16:  The  first  impossible  case. 
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Second,  suppose  both  rvi  and  rv2  intersect  with  owi  or  its  extension. 
By  the  similar  argument  as  the  previous  case,  we  claim  that  this  can  not 
happen. 

Third,  suppose  rvi  intersect  with  ow2  at  v3  and  rv2  intersect  with  owi 
at  v4.  Then  as  shown  in  Figure  4.17,  w4v3  and  w2v4  are  diagonal  lines  for  a 
quadrangle,  which  implies  that  their  intersection  lies  within  the  quadrangle. 
However,  since  we  assume  that  they  have  another  intersection  r  outside  the 
cone,  we  obtain  a  contradiction. 


Figure  4.17:  The  third  impossible  configuration. 

As  a  summary,  when  r  lies  outside  the  cone,  either  w4  or  w2  must  be 
visible  to  r.  ■ 

When  either  w4  or  w2  is  visible,  the  vehicle  has  to  distinguish  them 
from  all  detected  points.  A  candidate  w  for  w4  or  w2  satisfies  the  following 
necessary  conditions:  (1)  the  tangent  vector  to  r4  at  w  is  aligned  with  ow; 
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(2)  all  detected  points  are  located  on  one  side  of  the  line  ow,  with  r  lying  on 
the  other  side.  The  first  condition  can  be  easily  verified  because  the  tangent 
vector  at  w  and  the  line  ow  are  known.  As  declared  by  Claim  4.7.5,  the 
second  condition  is  a  sufficient  condition  for  w  G  owi  or  w  G  0W2  when 
there  is  only  one  obstacle  with  the  assumption  that  it  is  contained  in  a  cone. 
Therefore,  an  algorithm  to  construct  owj  and  ow2  is: 

Algorithm  4.7.7  Let  w  be  a  detected  point.  For  any  other  detected  point 
z,  let  z1  be  the  intersection  between  the  extended  ray  ow  and  the  extended 
ray  rz,  as  shown  in  Figure  4-18.  Then  if  ||  rz  ||  >  ||  rzi  ||  for  all  z,  then  w  is 
either  on  the  line  segment  owi  or  on  the  line  segment  ow2.  ow  is  the  virtual 
line  segment  we  want  to  construct. 


Figure  4.18:  An  algorithm  to  detect  virtual  boundary  owi. 
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Claim  4.7.8  A  vehicle  can  always  detect  a  segment  of  Ai,  the  boundary 
curve  of  Hi,  if  its  position  r  satisfies  r  ^  Hi. 

Proof 

When  r  is  outside  the  cone  formed  by  owi  and  owj,  we  know  that  it  can 
detect  either  the  virtual  boundary  owj  or  ow2.  When  r  is  inside  the  cone 
but  does  not  belong  to  H\ ,  it  can  detect  a  boundary  segment  of  Obi  which 
is  also  a  boundary  segment  of  Hi.  The  statement  is  proved.  ■ 

Theorem  4.7.9  Suppose  a  vehicle  satisfies  the  following  conditions: 

1.  The  initial  position  of  the  vehicle  does  not  belong  to  Hi. 

2.  The  vehicle  is  able  to  detect  virtual  boundary  segments  using  for  exam¬ 
ple,  algorithm  4-7.7. 

3.  The  vehicle  is  under  a  control  law  which  enable  it  to  follow  a  detected 
boundary  with  a  given  distance  in  unit  speed.  One  such  control  law  may 
be  the  second  control  law  in  the  boundary  following  section. 

Then  the  vehicle  is  able  to  follow  the  boundary  of  Hi  at  a  given  distance. 
Furthermore,  for  any  5  >  0,  there  exists  e  >  0  s.t.  by  following  the  boundary 
of  Hi  at  distance  e,  the  vehicle  is  able  to  get  within  the  <5  neighborhood  of  o 
in  finite  time. 

Proof 

Starting  outside  the  set  Hi,  the  vehicle  is  always  able  to  detect  the  boundary 
curve  of  Hi.  Because  the  vehicle  is  able  to  follow  a  detected  boundary,  it 
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is  able  to  follow  the  virtual  boundary  segments  too.  Since  the  boundary 
curve  of  H\  has  finite  length  and  the  vehicle  has  unit  speed,  by  following  the 
boundary  of  Hi  at  distance  small  enough,  the  vehicle  will  be  able  to  reach 
the  5  neighborhood  of  any  point  on  the  boundary  curve  Ai  in  finite  time. 
Since  o  e  A1;  the  statement  is  proved.  ■ 

4.7.3  Navigation  through  many  obstacles 

Adding  a  second  obstacle  to  the  environment  causes  several  difficulties.  The 
biggest  challenge  of  all  is  how  to  segment  the  detected  points,  that  is,  to 
determine  whether  two  detected  points  belong  to  the  same  obstacle  or  two 
obstacles.  Because  of  the  complexity  of  the  shape  of  each  obstacle  and  the 
many  possibilities  for  the  relative  position  between  the  vehicle  and  the  ob¬ 
stacles,  we  have  not  found  one  general  algorithm  which  is  successful  for  all 
cases.  However,  in  many  cases,  this  task  can  be  performed  by  combining 
several  methods.  Therefore,  it  is  reasonable  to  assume  that  segmentation 
can  be  successfully  achieved. 

Denote  the  obstacles  by  Obi  and  Ob2.  Under  the  assumption  that  we 
know  to  which  obstacle  a  detected  point  belongs,  we  may  use  Algorithm 
4.7.7  to  determine  the  tangent  points  wn,wi2  for  Obi  and  w2i,w22  for  Ob2 
depending  on  whether  one  or  more  of  them  are  visible  .  Then  we  may  con¬ 
struct  one  or  more  of  the  virtual  boundary  segments  owi;  for  i,j  =  1,2, 
therefore  partly  construct  Hi  and  H2. 
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Suppose  the  position  of  the  vehicle  r  lies  outside  the  set  H\  and  H2.  Then 
by  Claim  4.7.8,  the  vehicle  is  always  able  to  detect  a  boundary  segment.  We 
may  answer  two  questions.  Is  the  vehicle  still  able  to  follow  the  boundary  of 
H\  U  i/2?  And,  will  the  vehicle  be  able  to  enter  a  small  neighborhood  of  the 
target  by  following  the  boundary? 


Figure  4.19:  The  case  when  H\  fl  H2  =  {o},  two  virtual  boundary  segments 
own  and  ow2i  form  a  crack. 

In  order  to  understand  the  behavior  of  the  vehicle,  we  need  to  study  the 
following  cases: 

1.  i/i  Ci  H 2 ,  this  case  is  no  different  than  the  single  obstacle  case. 

2.  Hi  fl  H2  =  {o}.  In  this  case,  the  virtual  boundaries  intersect  at  target 
o  and  may  form  a  crack,  as  shown  in  Figure  4.19.  The  vehicle  is  able  to 
follow  the  combined  boundary  of  i/i  U  H2.  By  following  the  boundary,  the 
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vehicle  will  visit  a  neighborhood  of  the  target  and  then  departure  from  it  to 
come  back  again  later. 


Figure  4.20:  The  case  when  one  virtual  boundary  segment  own  intersects 
with  a  true  boundary  segment.  We  choose  the  direction  of  the  true  boundary 
so  that  the  boundary  of  H 1  U  H2  can  be  followed. 

3.  Hi  fl  H2  7^  0  with  case  1  and  2  excluded.  In  this  case,  at  least  one 
virtual  boundary  segments  of  one  obstacle  intersects  a  true  boundary  segment 
of  the  other  obstacle.  In  order  to  follow  the  combined  boundary  for  H\  U  H2) 
at  the  intersection  point,  the  vehicle  determines  the  direction  of  the  true 
boundary  curve  so  that  there  is  no  conflict  at  the  intersection  point.  Then 
the  combination  of  the  virtual  boundary  and  the  true  boundary  becomes  one 
segment  of  the  boundary  curve  for  Hi  U  H2,  as  shown  in  Figure  4.20.  The 
vehicle  will  be  able  to  follow  the  combined  boundary.  Furthermore,  since  o 
belongs  to  the  boundary  of  the  set  Hi  U  H2,  then  by  following  this  boundary, 
the  vehicle  will  arrive  in  a  small  neighborhood  of  the  target  in  finite  time. 

When  there  are  more  than  two  obstacles,  depends  on  the  shape  of  the 
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Figure  4.21:  The  case  when  the  boundary  of  the  set  U  does  not  contain 
the  target  o. 

obstacles,  there  does  exist  possibility  that  o  becomes  a  interior  point  of  the 
combined  set,  as  shown  in  Figure  4.21.  In  this  case  the  vehicle  will  not  get 
close  to  the  target  by  following  the  combined  boundary.  This  difficulty  is 
under  investigation  and  might  be  solved  by  different  ways  of  constructing 
virtual  boundary  segments. 

Based  on  the  discussions  for  two  obstacles,  the  following  theorem  states 
some  facts  about  the  case  when  there  are  more  than  two  obstacles. 

Theorem  4.7.10  Suppose  the  following  conditions  are  satisfied: 

1.  There  exist  finitely  many  obstacles  Obi;  i  =  1,2,...,  A7  satisfying  the 
assumptions  given  in  the  beginning  paragraph  of  this  section. 

2.  For  each  Obi,  the  vehicle  is  able  to  detect  boundary  segments  of  Hi  by 
using  algorithm  f.7.7. 
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3.  The  vehicle  is  able  to  track  boundary  curve  of  Hi  for  each  i  with  any 
given  distance  using  ,  for  example,  the  second  boundary  following  law. 

Then  starting  from  a  position  r (to)  ^  Uili  Hi,  the  vehicle  is  able  to  follow 
the  boundary  curve  of  the  set  Uli*.  Furthermore,  if  the  target  o  lies  on 
the  boundary  of  1J ^H,,  then  given  5  >  0,  there  exists  e  >  0  s.t.  the  5 
neighborhood  of  the  target  can  be  reached  by  the  vehicle  in  finite  time  if  the 
tracking  distance  between  the  vehicle  and  the  boundary  cure  is  less  than  e. 
Proof 

We  have  shown  the  case  for  two  obstacles.  We  shall  use  the  method  of 
proof  by  induction  to  prove  the  general  case.  We  suppose  the  statements  are 
true  when  there  are  IV  —  1  obstacles  and  use  this  as  the  induction  hypothesis. 

When  there  are  N  obstacles,  we  view  this  case  as  adding  one  more  obstacle 
to  the  case  of  N  —  1  obstacles.  We  may  construct  the  set  HN.  Very  similar 
to  the  case  when  there  are  only  two  obstacles,  we  can  find  three  possibilities: 

1.  H jv  C  U ^=1  Hi  or  1J ^=1  Hi  C  H n-  Then  there  is  no  difference  from 
the  case  of  N  —  1  obstacles  or  the  case  of  just  one  obstacles.  The  statements 
hold  by  the  induction  hypothesis. 

2.  Hn  fl  UiV  Hi  =  {o}.  This  case  is  similar  to  the  case  of  two  obsta¬ 
cles.  The  boundary  of  HN  will  be  appended  to  the  boundaries  of  H,. 

Because  the  vehicle  will  visit  the  5  neighborhood  of  the  target  in  finite  time 
by  following  the  boundary  of  H ^  with  distance  less  than  e^,  we  know  that 
by  following  the  boundaries  of  the  set  HN  U  Uii1  Hi  =  {o}  with  distance 
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less  than 


e  =  min{e1,e2, . . .  ,eN}  ,  (4.122) 

the  vehicle  will  visit  the  ^-neighborhood  of  the  target  in  finite  time. 

3.  H\  n  (J^1  Hi  7^  0,  but  case  1  and  2  is  excluded.  In  this  case,  in¬ 
tersections  of  virtual  boundary  segment  and  real  boundary  segment  exist. 
By  similar  argument  of  the  case  of  two  obstacles,  we  are  able  to  determine 
the  direction  of  the  true  boundary  so  that  the  combination  of  the  virtual 
boundary  and  the  true  boundary  becomes  one  part  of  the  boundary  curve 
for  Hn  U  (Jym1  Hi-  If  o  lies  on  the  boundary  of  (J)=i  H%  ,  by  following  this 
boundary,  the  vehicle  will  arrive  in  a  neighborhood  of  the  target  in  finite 
time. 
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Chapter  5 


Coordinated  Orbit  Transfer  of 
Satellite  Clusters 


There  is  increasing  interest  in  the  behaviors  of  a  cluster  of  relatively  small 
satellites  in  space.  Such  satellites  are  usually  inter-connected  by  wireless  ra¬ 
dio  or  laser  links  for  communication.  By  keeping  a  cluster  of  such  satellites 
in  a  certain  geometrical  form,  one  can  acquire  benefits  for  scientific  observa¬ 
tions.  The  information  sharing  across  the  cluster  will  allow  the  satellites  to 
work  cooperatively  to  perform  tasks  impossible  or  difficult  for  a  single  satel¬ 
lite.  Compared  to  a  single  satellite  providing  the  functionality  of  a  cluster,  a 
member  of  a  cluster  can  be  smaller/lighter.  Building  and  launching  costs  will 
then  be  reduced.  In  addition,  A  cluster  can  also  be  reconfigured  according 
to  different  mission  goals  or  in  the  case  that  a  member  of  the  cluster  fails. 

The  size  and  shape  of  a  cluster  or  formation  are  usually  determined  by 
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the  required  functionality  of  the  formation.  With  J2  and  higher  order  terms 
of  the  earth’s  gravitational  held  ignored,  the  solution  of  the  Kepler  problem 
tells  us  that  a  satellite  will  track  an  elliptical  or  circular  orbit.  To  determine 
the  orbit  of  each  satellite  in  a  cluster  such  that  the  size  and  shape  of  the  clus¬ 
ter  is  kept  unchanged  over  sufficiently  long  period  is  not  a  trivial  problem. 
Because  the  amount  of  fuel  on  board  is  limited  for  each  satellite,  one  needs 
to  find  a  set  of  orbits  that  demand  the  minimum  control  effort.  This  problem 
becomes  more  challenging  when  the  effect  of  J2  and  other  disturbances  are 
considered.  In  [13]  the  authors  proposed  a  ring  of  evenly  distributed  satellites 
on  the  same  circular  orbit  for  communication  purpose.  The  stability  of  such 
ring  is  proved  in  [19].  In  [6]  and  [29]  the  investigations  of  Clohessy- Wiltshire 
equations  revealed  possible  formations  with  constant  apparent  distributions. 
The  effects  of  perturbations  are  calculated  and  possible  station  keeping  strat¬ 
egy  are  proposed.  In  [31],  the  authors  proposed  that  in  the  presence  of  J2, 
a  set  of  constraints  on  the  orbital  elements  shall  be  satisfied  to  prevent  the 
orbits  from  drifting  apart.  However,  extra  station  keeping  is  still  necessary 
due  to  the  complicated  nature  of  the  disturbances.  The  adjustment  can  be 
performed  periodically  when  the  drifting  error  exceeds  certain  threshold. 

The  initialization  of  a  formation  is  another  important  problem.  The  whole 
cluster  can  be  launched  together  by  a  space  shuttle  or  rocket.  Satellites  will 
be  first  placed  in  a  parking  orbit  before  transferring  to  the  final  orbits  where 
desired  formation  will  be  achieved.  The  orbital  transfer  can  be  performed 
individually  using  a  control  law  based  on  a  Lyapunov  function  which  achieves 
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its  minimum  when  correct  orbit  is  reached.  In  [30],  the  Lyapunov  function 
is  expressed  as  a  quadratic  function  of  the  differences  of  orbital  elements 
between  current  orbit  and  the  destination  orbit.  In  [5],  the  authors  proved 
that  an  elliptic  orbit  can  be  represented  uniquely  as  a  point  on  the  linear  space 
formed  by  the  angular  momentum  vectors  and  Laplace  vectors.  A  Lyapunov 
function  was  built  naturally  from  the  Euclidean  metric  on  this  space.  It 
has  been  suggested  that  compared  to  Hohmann  transfer,  this  approach  will 
consume  less  fuel. 

The  problem  we  are  interested  in  is  a  cooperative  orbit  transfer  alogorithm 
to  achieve  a  satellite  formation.  What  we  want  to  consider  here  is  the  case 
when  members  of  the  formation  have  been  placed  relatively  far  apart.  They 
have  to  use  their  on-board  thrusters  to  get  to  the  desired  orbits  to  form 
the  desired  formation.  A  similar  case  is  when  the  whole  formation  has  to 
be  restructured  for  mission-related  reasons.  We  want  the  formation  to  be 
maintained  to  some  extent  during  the  transfer  and  be  re-established  after 
the  transfer. 

In  section  5.1,  we  develop  formulas  used  in  the  proofs  of  our  theorems.  In 
section  5.3,  a  brief  summary  of  results  in  [5]  and  [47]  are  given. We  introduce 
the  definition  of  periodic  satellite  formations  in  section  5.4.  Our  main  results 
and  proofs  about  orbit  transfer  of  periodic  formations  are  presented  in  section 
5.5.  Simulation  results  are  shown  in  section  5.7. 
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5.1  Control  osculating  elements 


If  the  mass  of  a  satellite  is  small  compared  to  the  mass  of  the  earth,  the 
Kepler  two  body  problem  can  be  approximated  by  a  one-center  problem  as: 

mq  —  — Wa  +  u  (5.1) 

where  q  G  'R?  is  the  position  vector  of  the  satellite  relative  to  the  center  of 
the  earth,  m  is  the  mass  of  the  satellite,  Vq  is  the  gravitational  potential  of 
the  earth,  u  is  the  control  force  plus  other  disturbances.  Without  considering 
higher  order  terms,  Vq  takes  the  form 

VG  =  (5.2) 

where  p  is  the  gravitational  constant.  Let  p  =  mq  be  the  momentum  vec¬ 
tor  of  the  satellite.  For  simplicity  we  assume  that  all  the  satellites 
considered  in  this  chapter  have  unit  mass. 

Let  us  make  the  following  definitions: 


lit) 

A(t) 

e(t) 

a(t ) 

cos  (E(t)) 
M(t) 


q(t)  y.  p(t) 
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hit)2 

M1  -  e(t)2) 

-(!-—) 

e(ty  a{ty 

E(t)  —  e(t)  sin (F(f)) 


(5.3) 
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where  h(t)  =  ||  l(t)  ||  and  r(t)  =  ||  q(t)  ||.  These  formulas  can  be  found  in 
textbooks  on  celestial  mechanics  [8].  I  is  the  angular  momentum  vector.  A 
is  called  the  Laplace  vector.  They  are  conserved  if  u(t)  =  0.  a  is  the  length 
of  the  semi-major  axis  and  e  is  the  eccentricity.  E  is  the  eccentric  anomaly 
and  M  is  the  mean  anomaly.  The  last  equation  is  Kepler’s  equation.  When 
e{t)  =  0,  the  eccentric  anomaly  E{t)  is  defined  to  be  M{t)  which  is  measured 
from  the  line  of  ascending  node.  For  now,  we  will  assume  that  e{t)  A  0. 

Notice  that  these  formulas  are  valid  for  all  t  and  all  the  elements  are 
differentiable  on  VA  x  VA  —  {0}.  So  we  can  take  derivative  on  both  sides  of 
equations  (5.3).  By  using  the  property  that  /,  A,  a  and  e  are  conserved  when 
u{t)  =  0,  we  have 


m 


dl  .  dl  . 
oq  op 

dl  .  31 , 

tw<7+  7 t-(-A/Vg  +  u) 
dq  dp 

dl 

“=0  ~dpU  ^ 

dl  ’ 

TPu{t)  ■ 


(5.4) 


Similarly,  we  have 

dA 

Mt )  =  .  (5.5) 

We  derive  the  perturbation  equations  for  the  orbital  elements  used  in 
our  control  algorithm.  The  control  forces  are  continuous  functions  of  time. 
Hence  the  derivatives  are  taken  along  the  controlled  dynamics  of  a  satellite. 
Part  of  the  orbital  elements  are  shown  in  figure  5.1. 
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Figure  5.1:  The  illustration  of  the  vectors  l,  A  and  orbital  elements  a, 
and  u. 

Starting  with  the  eccentricity,  we  have 


.  ,  A- A  A  dA  .  . 

e(t)  =  J4A\\  =  HMf '  ~diu{t) 

1  dA  T  ~ 

=  ~  vp )  A- u(t) 

p  op 

where  A  is  the  unit  vector  along  the  direction  of  A.  We  also  have 
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We  now  derive  the  formulas  for  M.  Since  r2  —  q  ■  q,  we  have  rr  = 


(5.6) 


(5.7) 


(5.8) 

q-p. 


172 


Hence 


On  the  other  hand 


Thus 


h2  =  l  ■  l  —  (q  x  p)  ■  (q  x  p) 
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Apply  r  =  a(l  —  e  cos(S)).  After  simplification,  we  have 

_ e  sin  (E) 

r  =  yTia - - - . 

Taking  derivatives  on  both  sides  of  r  =  a(  1  —  e  cos (E)),  we  have 
r  =  a(  1  —  e  cos(TJ))  +  a(e  cos (E)  —  e  sin(TJ)TJ)  . 


Thus, 
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On  the  other  hand,  taking  derivative  on  both  sides  of  M  =  E  —  e 
get 

M  =  E  —  e  sin(TJ)  —  e  cos (E)E 


(5.9) 

(5.10) 

(5.11) 

(5.12) 

(5.13) 

(5.14) 

sin (E),  we 
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(1  —  e  cos (E))E  —  e  sin (E)  . 


(5.15) 


Combining  this  equation  with  equation  (5.14),(5.8)and  (5.6),  we  have 
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(5.16) 


where  n  =  x/77  and 

V  a-3 


£M,£) 


2(1  —  e  cos(-E))2 
yuae(l  —  e2)  sin(iP) 
cos (E)  —  e  2(1  —  e  cos(if))2 
yue  sin(i?)  /i(l  —  e2)  sin(i?) 


(5.17) 


Notice  that  £  and  rj  will  be  oo  if  sin(£')  =  0.  Physically  it  means  that  when 
the  satellite  is  passing  perigee  or  apogee,  the  control  can  cause  a  sudden 
jump  of  the  mean  and  eccentric  anomalies.  In  order  to  prevent  this  from 
happening  in  our  control  laws,  we  will  turn  off  the  control  when  sin (E)  =  0. 

The  inclination  /  is  defined  as  the  angle  between  the  2-axis  and  the  an¬ 
gular  momentum  vector  l.  It  can  be  determined  by 


C0S(/)  =  \ . 


(5.18) 


Taking  derivatives  on  both  sides  we  get 


—  sin (/)  / 
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sin(J) 


cos (/)  sin(n),  — cos (/)  cos(f2),  —  sin(/)] /,  (5.19) 


because 


cos(f2)  = 


'l'l  +  11  h  sin (/) 


(5.20) 


Thus 


1  d  l 

I  =  —[cos (I)  sin(fl),  —  cos(J)  cos(f2),  —  sin(/)](— )u  . 


(5.21) 


The  argument  of  the  ascending  node  is  defined  as  the  angle  between 


the  x-  axis  and  the  ascending  node.  It  can  be  determined  by 


tan(f2)  —  ■ 

iy 

By  taking  derivatives  on  both  sides  of  equation  (5.22),  we  have 

1  A  lylx  Ixly 

COS2  (11)  /2 

1  ,  8 1 

=  o}-u. 

Let  n  =  A — L,  L,  0lr,  we  have 

•  h  sin (I)  cos2  (12)  .  d l 

a  =  - if - (di] 

1  (dl 

=  ;  •  77v(^~)  n-u  . 
h  sin(i)  ap 


(5.22) 


(5.23) 


(5.24) 


Notice  that  this  n  is  the  unit  vector  pointing  to  the  ascending  node. 

The  argument  of  the  perigee  c o  is  defined  as  the  angle  between  the  vector 
n  and  A.  It  can  be  determined  by 


cos(u;)  —  n  ■  A  . 


(5.25) 
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By  taking  derivatives  on  both  sides,  we  get 


to  = 


sm  cn 


-  (n  ■  A  +  A  ■  n)  . 


Because 


and 


A  =  —  (ld-AAT)A 
H  e 


n  = 


-(Id  —  nnT)n 


h  sin(J) 

where  Id  represents  the  identity  matrix,  one  can  verify  that 


(5.26) 


(5.27) 


(5.28) 


n  ■  A  —  Cl  cos (/)  sin(cn) 


(5.29) 


and 


~  _  1  _  1  ~ 

A  ■  n  —  — (n  —  cos(ca)A)  •  A  =  —  sinhu)^!.  •  A  (5.30) 

fie  fie 


where  A±  is  the  unit  vector  perpendicular  to  A  and  /  forming  an  acute  angle 
with  n.  The  derivative  of  lo  is 


cos  (I)  dl  T„  1  d  A  T~ 

u  =  ■  77T  )  n  ■  u - {— )tA±  ■  u 

/i  sm(i)  op  fie  op 


(5.31) 


The  longitude  parameter  6  is  used  in  stead  of  the  true  anomaly  when  the 
orbit  is  circular.  To  determine  9,  we  have  the  following 


Q  •  n  —  qx  cos(fl)  +  qy  sin(fl)  =  r  cos(d)  . 


(5.32) 


Hence 


cos (9)  =  —  cos(H)  +  —  sin(Q)  . 


(5.33) 
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On  the  other  hand, 


— q  x  n  —  r  sin(0) 


(5.34) 


This  gives  the  following  set  of  equations: 

l 

1 

l 


sin(0)  -T-  —  ~  sin(fl) 
h  r 


sin(0)  =  —  cos(fl) 
h  r 

sin(0)  -1  =  —  —  sin(O)  +  —  cos(h2) 

h  r  r 


(5.35) 


Thus,  6  can  be  uniquely  determined  by  using  equation  (5.33)  and  any  one  of 
equation  (5.35). 

We  now  calculate  the  derivative  of  9  which  contains  two  parts.  The  first 
part  is  due  to  the  orbital  motion  of  the  satellite  and  the  second  part  is  due 
to  the  perturbation  of  the  line  of  ascending  node  i.e. 


•  h  cos (/)  d l 
0  =  —  -  T  ■  n-u. 

rz  n  sm (7)  op 


(5.36) 


We  will  now  derive  equation  (5.120)  for  Mg.  We  start  from  the  cosine 
relationship  between  the  true  anomaly  /  and  the  eccentric  anomaly  E 

e  +  cos (/) 


cos  (E)  — 

1  +  e  cos (/) 

Taking  derivatives  on  both  sides  yields 


(5.37) 


—  sin(TJ)  E  = 


(e2  -  1)  sin (/) 


/  + 


sin  2(/) 


Because 


(1  +  e  cos (/))2  (l  +  ecos(/))2 

a/1  —  e2  sin(/) 


e  . 


sin(TJ)  = 


1  +  e  cos (/) 


(5.38) 


(5.39) 
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we  have 


E  = 


VT-> 


1  +  e  cos (/) 


/- 


sin (E)  . 

1  -e26 


1  —  e  cos  (if)  •  sin(i?) 

-/  -  v - ve 


We  know 


vi 


f  =  0-  u 


1-e2 


(5.40) 


(5.41) 


The  true  anomaly  of  the  ascending  node  is  (—o’).  Let  Eu  be  the  eccentric 
anomaly  of  the  ascending  node.  Its  derivative  is 


•  1-e  cos (Eu)  .  sin (Eu)  . 

E,„  = - ,  — l-w) - - - —e 


VT 


1-e2 


(5.42) 


Let  Mg  be  the  mean  anomaly  measured  from  the  line  of  ascending  node.  By 
using  the  Kepler’s  equation,  it  is  easy  to  see  that 


Mg  =  E  -  e  sin (E)  -  (E„  -  e  sin (Eu)) 


(5.43) 


Thus  the  derivative 

Mg  =  (1  —  e  cos (E))E  —  (1  —  e  cos (EJ))EU  —  e  (sin (E)  —  sin(Su,)) 
(1  —  e  cos(£'))2 


VT zrP 


-9  —  e  (sin (E)  —  sin (Ew)) 


sin(TJ)(l  —  e  cos (E))  sin(Sa;)(l  —  e  cos (EV)) 

6  1-e2  1-e2 

(1  —  e  cos(-E))2  —  (1  —  e  cos(EUJ))2 


VT ~ 


-u  . 


(5.44) 


We  know  that 


(1  —  e  cos (E))  =  -  . 

a 


(5.45) 
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Therefore, 


(1  —  e  cos (E))2  —  (1  —  e  cos 
VT ^ 

Because  of  equation  (5.31),  we  have 


r2  _  r2 
'  '  1L> 


r2  _  2 


=CU  = 


r2  _  ,r2 

'  '  w 


Since  we  already  know  that 


// \T  T  \ 
((yr-fdin-n) 


/ia2e  \/l  —  e2  <9p 
r2  -  r2  cos(J)  ^ 

a2V  1  -  e2  h  sin (/)  <9py 


n  ■  u 


(5.46) 


(5.47) 


6  =  —  —  0  cos  (/) 

1  .cM  T  ~ 

e  =  -(—)TA-u(t), 


(5.48) 


substitute  the  derivatives  in  equation  (5.44)  by  equations  (5.47)  and  (5.48), 
we  obtain 

r2  cos(J)  d  l 


Ma  =  n 


a2  a/1  -  e2  h  sin  ( / )  Op 
sin (E)  -  sin {EJ)  + 

OA 


n  ■  u 


r  sin (E)  -  r w  sin(Eal)  1  ,dA  T 


r2  _  2 
'  '  1L7 


a(l 


)-(— )M-u 

//  <9p 


/i  a2  e  \/l  —  e2  <9p 


((-s-JMx-u) 


+ 


rj™  *  _  ^ 


cos (/)  ,  01 .  T 


a2\/l  -  e2  h  sin (I)  dp 
which  simplifies  to 


n  ■  u 


(5.49) 


Mfl  =  n  — 


rz 


cos (/)  <9/  T, 


n  ■  u 


a2  \/l  -  e2  h  sin (I)  dpJ 

.  .  r  sin(£’)  —  rw  sm(EA .  1  ,cM.  T  ~ 

-(B,n(£)  -  sin(£„)  +  ia{1_;2)  "5-(^  ■ « 

<r>2  _  r)  A  ~ 

b  -  •*/)•  (5.50) 


/i  q  e 


dp 
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5.2  Shape  space  for  satellite  formations 


The  Jacobi  and  Kendall  shape  space  can  be  certainly  applied  to  study  the 
problems  of  satellite  formations.  However,  for  a  satellite  formation  near 
an  elliptic  orbit,  we  find  that  it  is  more  convenient  to  use  another  way  of 
describing  the  shape  of  satellite  formations. 

For  a  system  of  two  small  satellites,  one  can  make  the  following  assump¬ 
tions:  (a)  The  gravitational  attraction  between  the  satellites  can  be  omitted, 
(b)  The  total  mass  of  the  satellites  satisfies  m\  +  m2  <C  M  where  M  is  the 
mass  of  the  Earth.  Under  these  assumptions,  the  three  body  system  can  be 
approximated  by  two  uncoupled  two  body  problems.  Each  of  the  two  body 
problems  can  be  further  simplified  to  a  one  center  problem  with  the  center 
of  Earth  being  the  center  of  mass. 

Let  p  =  mq  be  the  momentum  of  the  satellite.  The  angular  momentum 
1  =  q  x  p  and  the  energy  W  =  Lm||g||2  +  V  are  integrals  of  motion  (i.e.  they 

is  also  conserved 

given  q(t)  ^  0  for  all  t  >  0. 

Knowing  l  ,  W  and  A,  we  have  seven  integrals  for  the  two-body  problem. 
They  are  not  all  independent  because  there  are  two  relations  connecting 
them.  They  are: 


are  conserved).  The  Laplace  vector  A  =  p  x  l  — 


A  ■  l  =  0 

A  ||2  =  m*n2  +  2mW\\l\\2  .  (5.51) 
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The  space  of  ordered  pairs  (l,  A)  is  R 3  x  R3  on  which  we  define  the  metric: 

d((h,  Ai),  (I2,  A2))  =  (||  h  —  ^2  ||  +  II  —  A2  ||  )5  .  (5.52) 

Let  P  denote  the  phase  space  of  the  satellite,  define  a  mapping  n  :  P  — > 
R3  x  R3,  ( q,p )  1— >  (l,  A).  Let  the  set  Se  be  defined  as 

se  =  {(q,p)  e  P\W{q,p)  <  0,1  ±  0}  (5.53) 

and  let  the  set  D  be  defined  as 

D  =  {(/,  A)  G  R3  x  R3  |  A  ■  l  =  0,  /  ^  0,  ||  A  ||  <  m2/u }  .  (5.54) 

In  [5],  the  authors  proved  the  following  results: 

Theorem  5.2.1  (Chang-Chichka-Marsden)The  following  hold: 

1.  Ee  is  the  union  of  all  elliptic  Keplerian  orbits. 

2.  7r(Se)  =  D  and  £e  =  7 \~l(D). 

3.  The  fiber  n~l  (/ ,  A)  is  a  unique  (oriented)  elliptic  Keplerian  orbit  for 
each  (/,  A)  e  D. 

The  mapping  7r  is  a  continuous  mapping  because  l  and  A  are  continuous  with 
respect  to  ( q,p ).  Furthermore,  the  following  corollary  hold. 

Corollary  5.2.2  7 r_1(/l)  is  compact  for  any  compact  set  K  C  D. 

In  the  case  of  two  satellites,  let  {li,Ai,Pi,Yiei)Di,'ni,di)  denote  the  cor¬ 
responding  objects  defined  for  the  ith  satellite.  Let  P  =  P\  x  P2  ,  q{t)  = 
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0?i(£),<?2 (t)),  pit))  =  ( p1(t),p2(t )).  Let  Ee  =  Eei  x  Se2.  Let  D  =  D1  x  D2 
and  I  =  (hjh),  A  =  [Ax,  A2).  Let 


d((h,  Ai),  (/2,  A2))  —  \J df  +  d%  (5.55) 

and  7r  =  H\  x  7r2.  We  define  the  space  of  elliptic  Keplerian  orbits  to  be  the 
set  D  with  the  distance  function  d. 

Proposition  5.2.3  The  following  hold: 

1.  Ee  is  the  union  of  all  pairs  of  elliptic  Keplerian  orbits. 

2.  7r(Se)  =  D  and  Ee  =  7f ~1{D). 

3.  The  fiber  i r_1(/,A)  is  a  unique  pair  of  (oriented)  elliptic  Keplerian 
orbit  for  each  (l,  A)  e  D. 

4 ■  7r_1(/l)  is  compact  for  any  compact  set  K  C  D. 

5.3  Orbit  transfer  of  single  satellite 

For  a  single  satellite  on  an  elliptic  orbit,  the  set  D  of  ordered  pairs  (/,  A)  is 
a  subset  of  VA  x  Kf  with  Euclidean  norm, 

D  =  {(Z,  A)  G  VA  x  VA  I  A  ■  l  =  0,  l  ^  o,  mil  <  m2p}  .  (5.56) 

To  control  the  orbit  transfer  of  a  single  satellite,  one  considers  a  Lyapunov 
function  from  [5] 

V(q.p)^\i\\l-lA'i‘  +  \\A-Adf)  (5.57) 
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where  (Id,  Ad)  is  the  pair  of  the  angular  momentum  vector  and  Laplace  vector 
of  the  target  elliptic  (circular)  orbit.  The  derivative  of  V  along  the  integral 
curve  of  the  system  is 

v  =  (i  -  id)  -i  +  (A  -  Ad)  ■  A 

—  [(^  ld)xq+ 

lx  (A  —  Ad)  +  (A  —  Ad)xpxq]  ■  u  .  (5.58) 

If  we  let  the  control  to  be 

u  =  -[(/  -  ld)xq  +  lx(A  -  Ad)  +  ((A  -  Ad)xp)xq]  ,  (5.59) 

then  V  <  0  along  the  trajectory  of  the  closed  loop  system.  The  following 
lemma  is  proved  in  [47]. 

Lemma  5.3.1  Suppose  a  single  satellite  has  external  control  u(t)  =  0.  Letx, 
y  be  time  invariant  unknown  vectors.  Suppose  (q(t),p(t))  e  Se?  the  solution 
of  equation 

xxq  +  Ixy  +  (yxp)xq  =  0  (5.60) 

is 

x  =  aA  y  —  al  (5.61) 

For  some  a  G  R 

We  will  give  a  proof  of  the  following  theorem  by  Chang,  Chichka  and  Marsden 
[5]: 
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Theorem  5.3.2  (Chang-Chichka-Marsden)  There  exists  c  >  0  such  that  if 
V(qo,Po )  <  c,  by  applying  the  control  law  as  in  equation  (5.59),  the  trajectory 
of  the  closed  loop  system  starting  at  ( g0,p0 )  will  asymptotically  converge  to 
the  target  orbit  Ti^l((ld,  Ad)) 

Proof  Let 

Q  =  {(l,A)  eTZ3  xTZ3\l-A  =  0}  .  (5.62) 

Recall  that  the  set  of  (/,  A)  pairs  for  elliptic  orbits  is 

D  =  {(/,  A)  e  TZ3  x  773|Z  ■  A  =  0,  l  ±  0,  ||  A  ||  <  m2p}  .  (5.63) 

It  is  easy  to  see  that  Q  is  a  closed  subset  of  TZ3  x  7 Z3  and  D  is  a  subset  of  Q 
with  nonempty  interior.  The  set 

B  =  {(/,  A)  e  TZ3  x  1Z3\V  <  c}  (5.64) 

is  a  closed  ball  in  TZ3  x  TZ3.  This  tells  us  that  the  set  B  n  Q  is  a  compact 
subset  of  TZ3  x  TZ3.  If  c  small  enough,  we  have  B  fl  Q  C  D,  then  by  corollary 
5.2.2,  the  set  n ~3(B  fl  Q)  will  be  a  compact  subset  of  Ee. 

The  condition  V(q0,p0)  <  c  tells  us  that  (qo,Po)  £  vr ~3(B  fl  Q).  By 
applying  LaSalle’s  invariance  principle, we  conclude  that  the  trajectory  of 
the  closed  loop  system  will  converge  to  the  maximal  invariant  set  within  the 
subset  of  BD  Q  where  V  =  0.  In  [47],  we  showed  that  the  maximal  invariant 
set  is  the  set  where  u(t)  =  0  for  all  t. 

According  to  lemma  5.3.1,  the  solution  to 

(l  -  ld)xq  +  lx(A-  Ad)  +  ((A  -  Ad)xpxq)  =  0  (5.65) 
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IS 


l  —  Id  =  ctA 

A  -  Ad  =  al  .  (5.66) 

Since  ld  ■  Ad  =  0  and  l  ■  A  =  0,  we  have 

a(||/||2  + ||A||2)  =  0.  (5.67) 

If  we  further  assumed  that 

c<^(\\ld\\2  +  \\Ad\\2)  .  (5.68) 

The  closed  loop  system  will  not  reach  the  set  where  l  =  0  and  A  =  0.  Thus 
the  only  possibility  for  equation  (5.67)  to  be  true  is  to  have  a  =  0.  So  the 
solution  is 

l  =  Id 

A  =  Ad  .  (5.69) 

■ 

The  authors  of  [5]  do  not  give  an  explicit  upper  bound  for  the  value  of  c. 
This  upper  bound  can  be  calculated  by  solving  a  constrained  maximization 
problem  as  below: 

SUP{^(II  /  -  Id  ||2  +  \\A  -  Ad  ||2)}  (5.70) 

under  constraints 

(l,  A)  E  Q,  m  0  and  ||  71 1|  <  m2n  (5-71) 
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First,  we  need  to  calculate  the  supremum  of  the  unconstrained  maxi¬ 
mization  problem.  It  is  easy  to  see  that  this  value  is  oo.  Then,  we  need 

1 1  2 

to  calculate  the  minimum  value  subject  to  l  =  0.  The  result  is  0.5  ||  ld  || 
achieved  when  A  =  Ad-  We  should  also  calculate  the  minimum  value  subject 
to  ||  A  ||  =  m2n,  by  applying  the  Lagrange  multiplier  method  we  found  this 
value  to  be  0.5 (m2/i—  ||  Ad  ||)2.  Hence,  for  theorem  5.3.2  to  be  valid,  we  must 
have 

c  <  min{0.5  ||  ld  ||2 , 0.5(m2yU  —  ||  Ad  ||)2}  (5-72) 

which  guarantees  (5.68). 

5.4  Periodic  formation 

Suppose  we  have  a  formation  consisting  of  m  satellites.  Let  Oj  denote  the 
orbit  of  the  jth  satellite.  The  orbit  Oj  and  the  position  of  the  satellite  on 
Oj  can  be  described  either  by  the  six  orbital  elements  (av  e3,  Iv  t3) 

or  by  ( lj,Aj,Tj )  where  a  denotes  the  length  of  semi-major  axis,  e  is  the 
eccentricity,  /  is  the  inclination,  is  the  longitude  of  the  ascending  node,  c o  is 
the  argument  of  the  perigee  and  r  is  the  perigee  passage  time.  If  Oj  is  elliptic 
or  circular,  except  for  some  singular  cases  when  some  of  the  orbital  elements 
are  not  well  defined,  theorem  5.2.1  tells  us  that  the  two  expressions  are 
equivalent.  Among  all  possible  formations,  we  are  interested  in  formations 
with  periodic  shape  changes.  We  can  make  the  following  definition: 
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Definition  5.4.1  A  formation  is  periodic  when  aj,  the  length  of  semi-major 
axis  of  orbit  Oj,  satisfies  aj  =  a  >  0  for  all  j  =  1,  2,  ...m. 

This  definition  is  valid  since  all  the  satellites  in  a  periodic  formation  will 
have  the  same  orbital  period 


2n\fcA 


(5.73) 


Thus  although  the  shape  of  the  formation  is  varying,  it  is  varying  periodically. 

On  the  other  hand,  given  a  set  of  orbits  with  the  identical  length  of  semi¬ 
major  axis,  there  are  infinitely  many  possible  periodic  formations.  We  need  to 
specify  their  relative  positions  on  the  orbits  to  determine  a  specific  formation. 
The  difficulty  is  that  the  relative  positions  are  complicated  functions  of  time. 
However,  the  differences  between  the  perigee  passage  times,  (r,  —  Tj),  are 
constants.  Because  the  mean  anomaly  is 


Mi  =  Ui(t  -  Ti )  (5.74) 

where  nt  =  2n/T  ,  then  (Mj  —  Mj )  are  constants.  By  specifying  the  values 
of  ( Ti  —  Tj )  or  (iff*  —  Mj)  for  all  i  and  j,  a  periodic  formation  can  be  uniquely 
determined. 


5.5  Transfer  of  satellite  formations  between 
elliptic  orbits 

With  all  the  tools  we  need  developed,  we  are  now  ready  to  determine  our 
control  laws.  To  illustrate  our  ideas,  we  will  show  the  calculations  for  two 
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satellites.  A  control  law  for  more  than  two  satellites  can  be  developed  based 
on  the  two-satellite  case  in  a  natural  way. 

To  set  up  a  periodic  formation  of  two  satellites,  one  can  control  each 
satellite  separately  to  transfer  to  its  target  orbit.  However,  this  will  not 
assure  the  correct  values  of  (t*  —  Tj )  or  (Mt  —  Mj).  In  order  to  do  that,  extra 
terms  involving  (r,  —  Tj)  or  (Mj  —  Mj)  should  be  added  in  the  summation  of 
the  Lyapunov  functions  for  single  satellites.  This  extension  will  result  in  a 
cooperative  orbit  transfer  of  multiple  satellites. 


We  introduce  a  variable  Tj  which  is  defined  as 


T 

l 

T 

x  i 


if  Ei  G  (— e,  7r  T  e) 

—  Mj),  if  Ei  G  (tt  —  e,  2,tc  T  e) 


(5.75) 


for  i  —  1,2,  where  ad  is  the  common  length  of  the  semi-major  axes  for  the 


destination  orbits. 


Here,  the  trouble  of  using  different  expressions  for  the  cases  Ei  G  (— e,  7T  + 
e)  and  Ei  G  (vr  — e,  27r  +  e)  is  caused  by  the  fact  that  Ei  G  S1.  Two  coordinate 
charts  are  required  on  S1.  Here  we  pick  the  charts  to  be 


ipi  :  (— e,  7T  +  e)  — >  (— e,  n  +  e)  s.t.  Ei  i— >•  E \ 

-02  :  (vr  —  e,  27t  +  e)  — >  (— e,  n  +  e)  s.t. Ei  i— >  {2t  —  Ei)  .  (5.76) 


Here,  the  value  of  e  is  chosen  so  that  the  two  satellites  will  always  be  in  the 
same  chart.  Because  in  a  satellite  formation  the  angular  separations  between 
satellites  are  usually  small,  the  value  of  e  is  small. 
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For  Ei  e  (— e,  7r  +  e),  we  have 


T;  = 


/  a  -  ■  3  [cl  .  ,  , 

}j  alMi  +  2\l  4a<Mi 

/ii  c)  A  ^ 

4 +p(k,Ai,Ei)(-^)TAi-ui(t ) 

opi 
f)  /. 

OPi 


(5.77) 


where 


C  \ja^lhA^Ei)+ 2^lp{l-Ei)Mi 


(5.78) 


For  Ei  e  (7T  —  e,  27r  +  e), 


T.t  =  -J^Mi  +  ^mdipn-Mi) 

\  ad  ^  \  ad 


opi 


(5.79) 


where 


C  -  -  A  /  Ei)  +  -  \[~n 


2  V  adM!  -e*2) 


(2vr  -  Mj) 


/ af  .  3  [a7  2 cue,  . 

P  =  "V  ^  +  2  V  /i(l  -  if) (2?r  “  Mi)  •  (5'80) 


Notice  that  we  have  terms  that  explicitly  contain  Mj.  If  not  handled 
well,  these  terms  will  cause  discontinuity  in  our  control  algorithm  when  the 
satellites  enter  a  new  chart.  The  reason  for  us  to  pick  the  particular  charts 
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(-01 ,  ^2)  is  to  reduce  the  discontinuities  in  the  derivatives  of  T *  caused  by 
changing  charts. 

We  will  design  a  Lyapunov  function  on  the  phase  space  of  the  two  satel¬ 
lites.  This  one  function  will  have  different  expressions  in  different  charts. 
The  Lyapunov  function  is 

Id  =  fdi  +  f/2  +  4sin(Tl~^2~^)2 
if  Ei  e  (— e,  7T  T  e) 

y  =  Vi  +  U  +  4sm(Tl  ~  +  ~)2 

if  Ei  e  (n  —  e,  2tt  +  e)  (5.81) 

where 

h'l  =  -(||  h  —  Idi  ||"  +  ||  dli  —  Ad\  ||") 

V2  =  -(II  h  —  ld2  ||2  +  II  A2  —  Ad2  ||2)  ■  (5.82) 

Here,  ( ldi,Adi )  and  (ld2,Ad2)  specify  the  orbits  in  a  two-satellite  periodic 

formation  and  (j)  specifies  the  desired  (Mi  —  M2)  on  these  orbits. 

We  can  calculate  the  derivative  of  V  as 

V  =  Vi+V2  +  sin(— !— ^-5-^)(t1  -  T2)  .  (5.83) 

The  choice  of  —  or  +  depends  on  the  value  of  Ei  as  in  the  definition  of  V . 
By  the  calculations  performed  in  the  single  satellite  case, 

h)  [(li  Adi) 

+  ((A  -  Adi)xpi)xqi]  ■  Ui  (5.84) 
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for  z  =  1,2.  Thus 


V  —  [(Zi  —  ldi  +  Ci  sin( — - - — - -)/i)xgi+ 

l\x(A\  —  Adi  +  Pi  sin( — - — - )Ai)  + 

(O^i  -Adi  +  Pi  sin( — - ^ — ~)Ai)xpi)xqi}  ■  «i 

T[(^2  —  ld.2  ~  C 2  sin( - ^ - )^)XQ,2  + 

l2X(A2  —  ^4^2  —  P2  sin( — - 2 - ~)^)  + 

((A2  -  Ad2  -  p2  sin(— - —  ^)A2)  xp2)xq2]  ■  u2  .  (5.85) 

In  order  to  get  V  <  0,  we  let 

Mi  =  -sin2(^i)[(/i  -  ld i  +  Ci  sin(Yl~^2:F0)/i)xgi  + 
h  x  (^4i  —  +  Pi  sin(Tl  J2T<^)Ai)  + 

((Ai  —  Adi  +  Pi  sin(Tl  ~^2T<^)Ai)xpi)xqi] 
u2  =  -sin2(E2)[(/2  -  ld2  -  C2  sin(Tl~^2T^)/2)xg2+ 
l2  x  (A2  —  Ad2  —  p2  sin(Tl  J2=f^)t42)+ 

((A2  -  Ad2  -  p2  sin {Tl~12T<l,)A2)xp2)xq2}  .  (5.86) 

Notice  that  the  factors  sin2(f?j)  cancel  the  term  sin (Ei)  in  the  denomina¬ 
tors  of  Ci  and  pi .  This  will  result  in  a  continuous  control  law  which  will  be  0 
when  Ei  =  0,  n. 

Another  issue  is  that  when  ex  =  0,  Ei  is  not  well  defined.  So  we  should  not 
allow  Adi  =  0.  Furthermore,  we  should  choose  our  initial  value  of  function  V 
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s.t.  the  subset  of  the  space  Eei  x  Ee2  where  Ai  =  0  will  not  be  reached  by 
the  controlled  dynamics. 

Let  z  =  (qi,Pi,q2,P2)-  We  now  proceed  to  find  the  initial  condition 
z0  =  (9i(0), pi (0),  92(2), p2(0))  for  z  s.t.  the  set 

SM  =  {z\V(z)  <  V(z0)}  (5.87) 

is  a  compact  subset  of  Eel  x  Se2  —  {z\Ai  =  0  or  A2  =  0}.  This  is  a  necessary 

step  because  we  want  to  apply  LaSalle’s  invariance  principle  to  prove  our 
main  result. 

Lemma  5.5.1  Let 

c  <  min{ci,c2}  (5.88) 

where 

Ci  =  min{^  ||  Adi  ||2  ,  ^  ||  ldi  ||2  ,  ^(p  -  ||  Adi  ||)2}  (5.89) 

for  i  =  1,  2.  Then  the  set 

SM  =  {z\V(z)  <  c}  (5.90) 

is  a  compact  subset  of  Eei  x  Se2  —  {z\A\  =  0orA2  =  0}. 

Proof  The  first  observation  is  that  the  set 

*S'i  =  {(9i,Pi)|l/(9i,Pi)  <  c*,c*  <  ci}  (5.91) 

is  a  subset  of  Eel  i.e.  Si  ft  Eel  =  Si. 
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In  fact,  Ci  is  the  supremum  of  V\(q\,p\)  on  the  set  Eel  —  {(<7i,Pi) \A\  =  0}. 
To  see  this,  we  solve  a  constrained  maximization  problem  as  below: 

^  —  ^ di  ||  +  ||  Ai  —  Adi  Id)}  (5.92) 

under  the  constraints 

A\  ■  l\  —  0  i|  /()  A\  7^  0  and  ||  Ai  ||  <  /x  .  (5.93) 

First,  we  need  to  calculate  the  supremum  of  the  unconstrained  maxi¬ 

mization  problem.  It  is  easy  to  see  that  this  value  is  oo.  Then,  we  need  to 
calculate  the  minimum  value  subject  to  l\  =  0.  The  result  is  1 1|  ld  ||2  achieved 
when  A  =  Ad.  Similarly,  the  minimum  value  subject  to  A\  =  0  is  ||  Ad  ||  . 
We  should  also  calculate  the  minimum  value  subject  to  ||  A  ||  =  /x.  By  apply¬ 
ing  the  Lagrange  multiplier  method  we  found  this  value  to  be  |(/x  —  ||  Ad  ||)2. 
Thus 

ci  =  min{ ^  ||  Adl  f  ,  ^  ||  ldl  ||2  ,  ^(/x  -  ||  Adl  ||)2}  (5.94) 

is  the  supremum  of  V\  on  the  set  Eei  —  {(qi,pi)\Ai  =  0}.  We  proved  that 

>Sj  C  Eei  -  {(q1,p1)\A1  =  0}. 

Another  observation  is  that  7r(Si)  is  a  compact  subset  of  D\ .  Thus  by 
corollary  5.2.2,  Si  is  a  compact  subset  of  Eei. 

We  can  make  the  same  arguments  for  the  case  when  i  =  2  to  prove  that 
S*2  is  a  compact  subset  of  Ee2  —  {(q,2,P2)|^2  =  0}. 

Hence  by  letting  c  <  minjci,  c2},  it  is  true  that 

Sm  C  Si  x  £2  C  Eei  x  Ee2  —  {z\Ai  =  0  or  A2  =  0}  .  (5.95) 
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Thus,  Sm  is  a  compact  subset  of  Eei  x  Ee2  —  {z\A\  =  0  or  A2  =  0}  ■ 

We  can  now  apply  LaSalle’s  invariance  principle  to  show  that  the  trajec¬ 
tory  of  the  closed  loop  system,  starting  within  Sm,  converges  to  the  maximal 
invariant  subset  of  Sm  where  u(t)  =  0  is  satisfied  for  all  t. 

Proposition  5.5.2  With  V ,c  and  Ui  given  as  in  (5.81)  ,(5.88)  and  (5.86), 
the  trajectory  starting  from  point  (qio,Pio,q2o,P2o)  which  satisfies 

V (Qio,  P 10,  <?20,  P20)  <  C  (5.96) 

will  converge  to  the  set  where 

li  Idi 
Ai  =  Adi 

(M1-M2)  =  <j>  (5.97) 

are  satisfied  for  i  —  1,  2. 

Proof  In  order  to  calculate  the  invariant  set, let  u i  =  0.  When  sin (Ei)  ^  0, 
we  get 

n  ,  ,  s  ■  /Ti-T27^,  n 

(h  ~  Idi  +  Ci  sm( - - - )h)xqi 

+/i  x  [A\  —  Ad i  +  pi  sin( - — - )Ai) 

+  ((Ai  -  Ad i  +  pi  sin(— — T - )Ai)xpi)xqi 
=  0  .  (5.98) 

Take  inner  products  on  both  sides  with  qi(t)  to  get 

(h  x  (Ai  -  Ad i  +  pi  sin(— — T  ^)Ai))  •  qi  =  0  .  (5.99) 
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This  is  equivalent  to 


(h  x  qi(t))  •  (Ai  -  Ad i  +  pi  sin(— — — -)Ai)  =  0  .  (5.100) 

Let 

B  =  Ai  —  Adi  +  pi  sin( - — - —)Ai  .  (5.101) 

Equation  (5.100)  means  B  is  perpendicular  to  the  vector  l\  x  qi(t).  We 
can  see  that  vector  B  should  stay  in  the  plane  spanned  by  l\  and  qiit)  as 
shown  in  Figure  5.2. 


Figure  5.2:  The  relationship  between  h,qi,  A\  —  Ad  and  B 

However,  from  the  assumption  that  U\(t)  =  0,  we  know  that  Ai  and  l\ 
are  constant  vectors.  The  time  varying  vector  B  will  sweep  a  line  segment 
passing  the  fixed  point  (Ai  —  Adi).  The  direction  of  this  line  segment  is 
aligned  with  A\.  So  vector  B  will  be  the  intersection  of  the  (h,qi)  plane 
and  this  line  segment.  Because  qi(t)  is  sweeping  the  orbital  plane,  the  (/1;  q\ ) 
plane  is  identical  at  t  and  t  +  kT\  where  k  is  an  integer  and  Tx  is  the  period 
of  the  first  satellite.  Since  the  line  segment  is  not  changed,  the  intersection 
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points  in  these  cases  must  be  identical.  Thus  we  must  have 

B(t)=B(t  +  VT1)  .  (5.102) 

Without  lost  of  generality,  suppose  at  time  t,  Ei(t),  E2(t )  G  (— e,  7r  +  e).  Then 
equation  (5.102)  requires  that 

=  pi  (Ei(t  +  tT,))  sin(  Tl|l+tTl)" Mt+tnt-t  j  (5.103) 

Let  k  —  1  in  equation  (5.103),  because  pi(Ei(t))  =  pi(£'i(i+T1)),  the  hrst 
observation  we  make  is  that  the  two  satellites  must  have  the  same  period.  In 
fact,  suppose  at  time  t0  equation  (5.103)  is  satisfied.  Then  at  time  to  +  T\, 
since  Ei(t0)  =  Ei(t0  +  Ti),Yi(t0)  =  T1(t0  +  T1)  and  all  the  angles(anomalies) 
are  in  the  range  of  [0,  2n),  we  must  have  Y2(t0)  =  Y2(t0  +  Ti).  But 

T2(to)  -  r2(t0  +  Ti)  =  -Mmto  +  7\)  -  M2(to))  .  (5.104) 

V  ad 

Then  T2(t0)  =  T2(t0  +  Tf)  will  be  satisfied  only  if  M2(t0  +  Ti)  =  M2(t0). 
Thus  we  shall  have  T|  =  k\T2  where  k\  is  a  positive  integer.  Remember  we 
can  apply  the  same  argument  to  the  second  satellite  to  get  T2  =  k\T\ .  Thus 
we  must  have  T\  —  T2.  Hence  on  the  invariant  set,  we  proved  that  a±  =  a2. 

On  the  other  hand,  for  a  specific  time  t,  we  know  that  there  exists  t*  e 
[0,  T()  such  that 

n  +  fi{t)=fi{t  +  t*)  (5.105) 

where  f\  is  the  true  anomaly  of  the  hrst  satellite.  The  value  of  t*  depends  on 
t.  The  plane  spanned  by  at  time  t  will  also  be  identical  to  the  plane 
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spanned  by  (li,  qx )  at  time  t  +  t*.  Thus  we  must  have 


B{t)  =  B{t  +  t*) 


(5.106) 


which  requires  that 


Pi(El(t))  sm(MhMl^) 

=  pi(^i(t  +  r))  sin(Tl(t+n^2(w*)+0) 


(5.107) 


Further,  cq  =  a2  implies  that  Mx  (t)  —  M2(t)  =  Mx  (t  +  t*)  —  M2(t  +  t*), 
one  can  verify  that 


Tift+g)  -  T2(t  + 1*)  + 


2  7  ”^v  2 

For  (5.107)  to  be  satisfied,  one  possibility  is  that 

.  ,T!(*)-T 2(t)-</> 


(5.108) 


Sill 


=  0  . 


(5.109) 


Another  possibility  is  that 


Pi(Ei(t))  —  —  pi(Ei(t  +  t*))  .  (5.110) 

By  the  definition  of  px ,  one  can  verify  that  (5.110)  can  only  be  satisfied  when 
t  takes  value  from  a  set  of  measure  0.  Thus,  for  (5.107)  to  be  satisfied,  (5.109) 
must  be  true. 

Because  of  (5.109),  the  time  varying  parts  in  equation  (5.98)  vanish.  We 
can  make  the  same  argument  as  in  the  proof  of  the  single  satellite  case  [5]  to 
show  that 


h  —  Idl 
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(5.111) 


Ai  —  Adi  ■ 

We  can  apply  similar  arguments  for  the  second  satellite. 
Thus  we  have 


h  Idi 

Ai  =  Adi 

(Ti-T2)  =  ±<j> 


(5.112) 


for  i  =  1,  2.  By  the  definition  of  Yj  and  T2  in  equation(5.75),  we  have 

4mi-aMm2  =  0.  (5.113) 

V  ad 

But  we  already  know  a\  =  a2  =  ad,  so  we  conclude  that 


(Mi  -  .\I2)  -  o 


(5.114) 


This  proposition  can  be  easily  extended  to  m  satellites  where  m  >  2.  Let 
the  mth  satellite  be  the  leader  of  the  formation.  Let 

l/^f;(r,+4sm(T-~T4roTA)2)  (5.115) 

i 

where  i  =  1,2,  3, ...,  m  ,  c i>m  =  0  and 

Vi  —  ^||  h  ~  Idi  ||  +  ||  Ai  —  Adi  ||  •  (5.116) 

It  is  easy  to  see  that  the  control  for  the  ith  satellite  with  i  <  m  —  1  is  the 
same  as  the  control  for  the  (i,m)  pair  of  satellites.  The  initial  value  of  V 
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should  be  less  than  the  minimum  value  of  c,  for  i  =  1,  2, (m  —  1).  Then 
the  solution  will  converge  to  the  set  where 

1%  Idi 

A  i  =  Adi 

Mi  —  Mn  =  (j)i  (5.117) 

are  satisfied  for  i  =  1,  2, m. 

5.6  Transfer  of  satellite  formations  to  circu¬ 
lar  orbits 

Formations  on  circular  orbits  are  of  great  importance  as  addressed  in  [19]. 
If  the  destination  of  at  least  one  of  the  satellites  in  a  formation  is  circular, 
we  will  have  difficulty  to  determine  the  mean  anomaly.  Since  the  Laplace 
vector  A  vanishes,  the  perigee  point  can  not  be  determined.  In  such  a  case, 
the  position  of  a  satellite  on  the  circular  orbit  is  usually  described  by  using 
the  argument  of  latitude  defined  as  the  angle  6  between  the  line  of  ascending 
node  and  the  position  vector  q.  It  is  easy  to  see  that  on  an  elliptic  orbit,  this 
angle 

d  =  u  +  f  (5.118) 

where  u  is  the  argument  of  perigee  and  /  is  the  true  anomaly.  On  a  circular 
orb  it,  we  have 

6  =  n(t-r )  (5.119) 
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where  r  is  the  time  when  the  satellite  passing  the  ascending  node  for  the  first 
time.  As  we  can  see,  9  serves  as  the  mean  anomaly  for  circular  orbit. 

Although  9  and  9  can  be  easily  determined  as  discussed  in  the  appendix, 
they  are  not  suitable  for  our  Lyapunov  function  based  control  laws  because 
9  is  not  affine  in  time  on  elliptic  orbit.  Thus,  we  will  use  the  angle  Mg  which 
is  the  mean  anomaly  measured  from  the  line  of  node.  Mg  can  be  determined 
from  9.  But  if  the  orbit  is  close  to  circular,  we  can  use  9  as  an  approximation 
for  Mg.  Further  analysis  in  the  appendix  shows  that  the  derivative  of  Mg  is 

•  r?„  cos (I)  ,dl.T~ 

Me  =  n - ,w  ,  —  Yn-u 

a 2  a/1  -  e2  h  sin (Iydp 

.  .  _  . _  .  r  sin(£0  —  rw  sin  (i±L) .  1  ,dA.  T  ~ 

-(me)  -  me.)  +  aj(1_;2)  -)-^)Ta  ■ « 

_  r2  E)  A 

+  2  /t^((7-)TA±  ■  u)  .  (5.120) 

/ra2  e  vl  —  e2  op 

Here,  u  is  the  argument  of  perigee,  E  and  Ew  are  the  eccentric  anomalies 
corresponding  to  9  —  u  and  u,  denote  the  distance  between  the  ascending 
node  and  the  gravitational  center.  We  let  n  =  —L=[—ly,  lx,  0]T,  this  n  is 
the  unit  vector  pointing  to  the  ascending  node.  We  let  Aj_  be  the  unit  vector 
perpendicular  to  A  and  l  forming  an  acute  angle  with  n. 

We  will  now  study  the  case  when  we  want  to  setup  a  formation  of  which 
all  the  members  are  on  circular  orbits  with  the  same  radius.  According  to 
the  discussions  made  in  section  5.4,  this  formation  is  a  periodic  formation. 
Let  Tj  be  the  time  when  the  ith  satellite  passes  the  line  of  ascending  node 
for  the  first  time.  Similar  to  the  perigee  passage  time  in  our  elliptic  orbit 
case,  Ti  is  a  constant  of  the  free  motion  for  satellite  i.  Hence  by  specifying 


200 


the  differences  r*  —  r3  or  9{  —  9j  between  satellites,  a  periodic  formation  on 
circular  orbits  can  be  uniquely  determined. 

Similar  to  the  elliptic  orbit  case,  we  introduce  a  variable  dq  which  is 
defined  as 


Vt  = 


= 


M0i,  ^  e  (-e,7r  +  e) 


(27 r  -  Mdi),  if  9i  G  (7T  -  e,  27T  +  e) 


(5.121) 


for  i  —  1,2,  where  is  the  common  length  of  the  semi-major  axes  (or  radius) 
for  the  destination  orbits.  Here,  the  reasons  for  using  two  charts  are  the  same 
as  in  the  case  of  elliptic  orbits. 

Calculations  in  the  appendix  show  that  the  derivative  of  'b  is 


V 


dl 


dA 


+  (w-)  (An  +  nl)  ■  u  +  (— )  (a A  +  gA±)  ■  u 


'dp 


dp 


(5.122) 


where  for  9  G  (— e,  7T  +  e), 


A 


K 


a 


cos (/) 
h  sin (/) 


.  ,  „  N  ,  r  sin (£)  -  r ^  sin(^) 

-  + - ^5) - 

r  sin(£’)  -  ru  sin(Ew) 
a(l  -e2)  j 
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I  a3  r2  —  r2 


(if  //  a 2  e  \/l  —  e2 
[a3 2  (cos  (-E)  —  cos(£?w)) 


(5.123) 


and  for  0  G  (7r  —  e,  27t  +  e), 


A  = 


'a3  cos(J) 

a3j  a2  \/\  -  e2h  sin (I) 

^a3" cos(/) 
a()  a2  h  sin (/) 


K  //a(l  -  e2)  * 


/r(!  —  e2)  ^  +  // V  a3d 


1  /a3/  •  /pn  •  /P  x  ,  r  sin(E)  -  ry,  am(Eu)< 
-\  -(sm(E)  -  sm{Eu)  + - — - - , 


a(l  —  e2) 


1  /a3  /  •  /Px  •  /P  w  r  sin (£)  -rw  sin(£?w). 

Hpd{sm{E)  ~  + - ^(T^) - 

/a3"  r2  — 


y  apa2eV  1  -  e2 
a3  2(cos (i?)  —  cos(-E^)) 


(5.124) 


Here  we  use  the  when  the  eccentricity  of  the  orbits  are  very  small.  When 
the  orbit  is  circular,  E  and  Ew  is  not  well  defined,  but  we  have  Mg  =  6  and 


a  cos(J)  ,  91 

0  =  n-  .  77r(^~)  n-u. 
ftsm(i)  (7J9 


(5.125) 


Thus  when  e  =  0 


a3  cos (/) 
h  sin (/) 

K  =  /xa(l  -  e2)  * 
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a  —  0 

0  =  0.  (5.126) 


This  result  agree  with  the  limits  of  (5.123)  and  (5.124)  as  e  — >  0. 

Mimic  what  has  been  done  in  the  elliptic  orbit  case,  we  design  a  Lyapunov 
function  on  the  phase  space  of  two  satellites  as 


V 


V 


V1  +  V2  +  4  sin( 
if  9t  G  (— e,  7T  +  e) 

U  +  V2  +  4  sm(^~^  +  b2 
if  dj  G  (7r  —  e,  2n  +  e) 


(5.127) 


where 

Vi  —  2  (II  ^  —  ^dl  II"  +  II  ^1  ||2) 

V2  =  \i\\h  -  U2W2  +  \\A2\\2)  .  (5.128) 

The  derivative  of  this  function  has  similar  form  as  equation  (5.85).  And  the 
control  law  u  s.t.  V  <  0  can  be  chosen  as 


U\  —  —[(A  —  Idi  +  sin('1’1  ^2T<^)(Aini  +  Kih))xqi+ 
hx(Ai  +  sin(—  ^2=F^)((JiAi  +  ftAu))+ 

((Ai  +  sin(^|^)((JiAi  +  f?iAi_L)xp1)xg1] 

u2  =  —[{h  ~  ld,2  —  sin(^1  ^2T<^)(X2n2  +  k2 l2))xq2+ 
l2x(A2  —  sin(—  ^ 2T<^)(cr2A2  +  q2A2^_))+ 

((A2  -  s in(^i=|2l^)(o-2A2  +  ^2A2_L))xp2)xg2]  •  (5.129) 
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Based  on  Lemma  5.5.1,  the  following  lemma  is  obvious. 

Lemma  5.6.1  Let 

c  <  min{ci,c2}  (5.130) 

where 

d  =  minji  ||  ldi  ||2  ,  ^p2}  (5.131) 

for  i  =  1,  2.  Then  the  set 

S M  =  {z\V{z)  <  c}  (5.132) 

is  a  compact  subset  of  Eei  x  Se2. 

Hence  we  will  prove  that  starting  within  the  set  Sm,  the  controlled  dy¬ 
namics  will  converge  to  the  maximal  invariant  set  where  formation  relation¬ 
ships  are  obtained. 

Proposition  5.6.2  With  V ,c  andu.i  given  as  in  (5.127)  ,(5.130)  and  (5.129), 
the  trajectory  starting  from  point  (?io,Pio,  Q201P20)  which  satisfies 

V(qw,Pio,q20,P2o)  <  c  (5.133) 

will  converge  to  the  set  where 

li  ldi 

A  =  0 

(0i -02)  =  </>  (5.134) 

are  satisfied  for  i  —  1,  2. 
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Proof  Let  u \  =  0,  and  take  the  inner  product  with  q\  on  both  sides  of  the 
equation,  we  get 

(llxB1)-q1=  0  (5.135) 

where 

Ti  —  vJ/9  =r  x  ~  ~ 

B\  =  A1  +  sin( - — - )(cr1A1  +  ftdu)  .  (5.136) 

At  the  time  t0  when  Ei(t0)  =  E^i,  since  Mg\  =  0  and  rq  =  rw i,  we  have 


Qi(to)  =  0  and  <Ti (t0)  =  0  , 


(5.137) 


hence 

B\ (to)  —  Ai  . 

On  the  other  hand, 

( qi  x  h)  ■  Bi  —  0 


(5.138) 


(5.139) 


tells  us  that  Bi  lies  in  the  plane  spanned  by  /i  and  q\ .  Thus,  when  gi(to)  is 
not  aligned  with  Ai,  Bi(to)  must  vanish.  This  implies  that 


Ai  =  0  . 


(5.140) 


But  it  is  also  possible  that  Q'i(to)  is  aligned  with  A\.  In  this  case  we  have 

E^i  =  0  and  u>i  —  0  .  (5.141) 


Therefore,  for  all  time  t, 


Qi  if) 
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ai(t) 


3  e\ 


Mi  -  ei) 


^1 - \  /  — i-  Sin(-E'i)  (1  + 


r  i 


ai(l  -  e\)‘ 


(5.142) 


Thus, 

B\(t)  —  A\  +  sin( — - — - )(oqHi  +  £>i^4i_l)  ,  (5.143) 

and  Biit)  has  to  be  aligned  with  qi(t)  for  all  time  t.  If  we  let  T)  to  be  the 
orbital  period  of  the  first  satellite  and  T2  be  the  orbital  period  of  the  second 
satellite,  then  after  one  period  T\  the  first  satellite  will  return  to  the  same 
position,  which  implies  that 


Ai  +  sin( 


Mt  +  Ti) 


—  Ai  +  sin 


*i(t)  ~ 


^2(t  +  ri)T</>)(ai(t  +  T1)A1  +  ei(t  +  Ti)Ai±) 
!2(t)^)(ai(l)ii  +  gi(t)Ai±)  .  (5.144) 


Because  Me1{t)  =  Mg1(t  +  T\)  and  Exit)  =  Ei(t  +  Ti),  we  know  cri(t  +  Ti)  = 
(j i (t)  and  Qi(t  +  Ti)  =  Qi(t).  Then  the  above  equation  results  in 


sin( 


sin( 


{I  ~  I\)  ~  <;> 


(5.145) 


Therefore  we  must  have 


Mq2 (t)  —  Mq2 (t  +  Ti)  .  (5.146) 

So  T1/T2  is  a  positive  integer.  On  the  other  hand,  the  above  arguments  can 
be  applied  to  the  second  satellite.  If  A2  7^  0,  T2/T\  must  be  a  positive  integer. 
Hence  T2  =  T\  which  implies  that  a\  =  a2. 
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Still  assuming  qi(t0)  is  aligned  with  Ai,  we  know  that  by  the  property  of 
elliptic  orbit,  after  half  period,  qi(t0  +  Ti/2)  is  aligned  with  Ai  again,  which 
means  that 


.  tfi(t0  +  Ti/2) 

sm( - 


^2(t0  +  Ti/2)  =f  (p 
2 


)Qi(to  +  Ti/2 ) 


0. 


(5.147) 


According  to  the  definition  of  4',  one  can  verify  that 
.  ,'h1(t  +  Ti/2)  —  T2(f  +  Ti/2)  =F  0N.  . 

sm( - - - )  =  -sm( - - - )  • 

(5.148) 

Therefore,  the  hrst  possibility  is  that 


6i{to  +  T1/2)  =  -ei{to)  =  0.  (5.149) 


This  further  implies  that  ri(f0  +  Ti/2)  =  ri(f0)-  Because  at  time  f0  the 
satellite  is  at  perigee  and  at  time  t0  +  Tf/ 2  the  satellite  is  at  apogee,  we 
conclude  that  the  orbit  is  circular  i.e.  A\  =  0. 

The  second  possibility  for  equation  (5.147)  to  hold  is  that 


sin( 


*i{to) 


^2(t0)  T0 

2 


(5.150) 


But  because  Tf  —  T2,  we  must  have 


d'l  (t)  —  ^(f)  —  4'1(t0)  —  ^2(^0)  (5.151) 


which  implies  that 


^(1)7^ 

2 


(5.152) 
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One  can  then  make  the  same  argument  as  in  the  proof  of  the  single  satellite 
case  [5]  to  show  that 

h  =  Oi 

Ax  =  0  (5.153) 

Up  to  this  point,  we  have  proved  that  our  algorithm  will  cause  either  A\ 
or  A2  to  vanish.  Without  loss  of  generality,  we  proceed  by  assuming  A2  =  0. 
Then  from  U\  =  0,  we  get 

(h  -  Idi  +  sin(— — —  ~)(AiWi  +  Kih))xq1  =  0  .  (5.154) 

Let 

D1=l1-  ld i  +  sin( - — - )(AiUi  +  Kxh)  .  (5.155) 

Such  Di  must  be  aligned  with  qi  and  also  lies  in  the  plane  passing  (/x  —  /rfl) 
spanned  by  l\  and  hi .  Because  the  plane  is  fixed  and  qi  is  periodic  in  time, 
at  time  t  and  time  ( t  +  T)),  the  intersection  of  q\  and  the  plane  must  be 
the  same.  This  tells  us  that  T\  /T2  is  a  positive  integer.  Now  if  A2  =  0,  the 
above  arguments  can  be  repeated  so  that  we  conclude  T2/T\  is  also  a  positive 
integer.  If  A2  ^  0,  we  can  repeat  the  arguments  after  equation  (5.141)  to 
conclude  that  T2/Ti  is  also  a  positive  integer.  Therefore,  we  conclude  that 
Ti  =  T2  and  cq  =  a2 

According  to  the  property  for  circular  orbit,  at  time  t  and  (t  +  Ti/2),  the 
intersection  of  q\  and  the  plane  spanned  by  l\  and  q2  passing  l\  —  ld  should 
be  the  same,  i.e. 

D1{t)=D1{t  +  T1/ 2).  (5.156) 
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Thus 


sin( 

sin( 


Ti(t)  -  V2(t)  T0 


){Xi(t)ni  +  Ki(t)/i)  = 


*i{t  +  Ti/2)  -  T2(t  +  Ti/2)  =F  0 


)(Ai(t  +  Ti/2)ni 


+K\{t  +  Ti/2)l\)  . 


(5.157) 


Because  of  equation  (5.148),  equation  (5.157)  are  satisfied  only  if 


sin( 


Ti(f) 


jMg)  T0 
2 


(5.158) 


or 

Ai(t)ni  +  Ki(t)li  =  —  Ai(f  +  Ti/2)n1  —  Ki(t  +  Ti/2)/!  .  (5.159) 

Equation  (5.159)  implies  that  for  all  time  t, 


n(t)  =  — K,(t  +  Ti/2) 


(5.160) 


which  further  implies  that 

MeAt)  =  \  • 

This  is  impossible.  Thus,  the  only  possibility  is  to  have 


sin( 


*i(t) 


^2(t)  T0 

2 


(5.161) 


(5.162) 


Because  of  (5.162),  the  time  varying  parts  in  equation  u\  —  0  vanish.  We 
can  make  the  same  argument  as  in  the  proof  of  the  single  satellite  case  [5]  to 
show  that 


h  —  IdX 
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(5.163) 


Ax  =  0. 

Now  let  U2(t)  =  0  and  substitute  equation  (5.162)  into  it.  We  conclude  that 
the  second  satellite  will  achieve 


Thus  we  have 


h  —  ld2 
A2  =  0. 


H  ^ di 

Ai  =  0 

(Ti  -  T2)  =  ±(j) 


(5.164) 


(5.165) 


for  i  =  1,2.  By  the  definition  of  Ti  and  T2  in  equation(5.121),  we  have 


-  \l  if  M®2  = 


a 


“d  V 

But  we  already  know  a\  =  a2  =  ad  and  the  orbits  are  circular,  we  conclude 
that 

(01-02)  =  <p.  (5.167) 


(5.166) 


We  would  like  to  mention  that  this  algorithm  is  not  limited  to  set  up 
formations  on  circular  orbits.  With  slight  modification  to  the  proof  one  can 
show  the  convergence  result  hold  for  elliptic  orbits. 
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5.7  Simulation  results 


To  verify  our  algorithms,  a  series  of  simulations  have  been  carried  out  in 
Matlab.  First  we  show  a  controlled  transfer  of  two  satellites  from  orbit 
[a,  e,  i,  ou,  fl]  =  [20,  0.1,  7t/4,  7t/2,  0]  with  initial  separation  of  mean  anomaly 
being  7t/90  to  the  orbit  [a,  e,  i,  to,  ff]  =  [25,  0.05,  7t/3,  7t/2,  0]  with  final  sepa¬ 
ration  of  mean  anomaly  being  7t/18  ~  0.175.  Only  relative  motion  between 
the  satellites  is  plotted  in  an  inertial  frame  centered  at  one  of  the  satellites. 
Figure  5.3  displays  the  desired  relative  motion  between  the  satellites.  Fig¬ 
ure  5.4  displays  the  relative  motion  between  the  satellites  using  the  control 
algorithm  proposed.  The  final  orbits  for  the  two  satellites  are: 

[ai,ei,/i,o;i,fii]  =  [25.10,  0.0495,  1.0472,  1.6231,  0] 

[a2,e2,/2,a; 2,ft2]  =  [25.12,  0.0507,  1.0472,  1.6231,  0] 

and  the  final  separation  of  mean  anomaly  is  0.168.  As  we  can  see,  the  desired 
orbit  and  separation  are  achieved  with  small  error.  We  noticed  that  by 
adjusting  the  weight  of  the  terms  in  our  Lyapunov  functions,  we  can  control 
the  distribution  of  errors  among  the  orbital  elements  of  final  orbits. 

Similar  experiments  are  performed  to  set  up  a  two  satellite  formation 
in  circular  orbits.  Initially,  the  two  satellites  are  on  an  elliptic  orbit  with 
[a,  e,  i,  lu,  Q]  =  [20,  0.1,  7t/4,  tt/2,  0]  and  the  separation  of  argument  of  latitude 
is  7r / 90.  Their  destination  orbit  is  circular  with  [a,  e,  i,  O]  =  [25,  0,  7t/3,  7t/4] 
and  the  separation  7t/18.  Using  our  algorithm,  the  final  orbits  are 

[ai,ei,Ji,fti]  =  [25.082,0.0023,1.0472,0.7854] 
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-2  -4 


Figure  5.4:  The  relative  motion  achieved  by  our  algorithm. 
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[«2,  62,  ^2, 


[25.078,  0.0022,  1.0472,  0.7854] 


and  the  final  separation  is  0.174.  Again,  the  formation  is  achieved  with  small 
error. 
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Chapter  6 


Summary  and  Future 
Directions 


We  have  answered  several  questions  related  to  how  to  control  the  formation 
of  a  team  of  robots.  We  model  the  robots  as  particles  and  use  various  shape 
theory  to  describe  small  formations  in  different  contexts.  Jacobi  shape  theory 
is  the  first  such  theory,  which  inspired  the  creation  of  shape  theory  on  Lie 
groups  in  chapter  4  and  shape  theory  on  momentum  maps  in  chapter  5.  Of 
course,  in  order  to  understand  complex  teaming  behaviors,  especially  when 
large  number  of  objects  are  involved,  such  theories  need  to  be  significantly 
extended  if  still  applicable. 

Shape  theory  tells  us  what  the  system  equations  are  for  a  formation.  The 
equations  often  describe  the  dynamics  of  the  shape  only,  which  gives  us  an 
advantage  of  designing  control  laws  using  quantities  that  are  independent  of 
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choice  of  coordinate  frame.  In  chapter  3,  we  tried  to  justify  that  this  is  an 
advantage  that  should  not  be  neglected.  By  using  gauge  covariant  controllers, 
one  can  improve  the  robustness  of  the  formation  and,  at  the  same  time, 
reduce  the  burden  on  communication  links.  These  results  might  shed  some 
light  on  how  to  cooperate  more  effectively.  We  admit  that  communication 
theory  shall  also  play  an  important  role  here. 

Chapter  2  fully  illustrated  the  strength  of  shape  theory.  The  fact  that 
navigation  problem  can  be  considered  as  a  formation  control  problem  brings 
a  new  perspective.  This  is  supported  by  experimental  data  in  [50].  We  have 
obtained  a  series  of  results  that  improve  the  known  navigation  algorithms. 
Although  only  the  2D  case  has  been  fully  developed  in  this  dissertation,  we 
have  obtained  3D  results  in  very  similar  ways  [46] .  Besides,  some  preliminary 
results  toward  navigation  for  formations  have  also  been  developed [49].  The 
potential  of  this  approach  for  navigation  is  yet  to  be  fully  explored.  Mean¬ 
while,  this  approach  also  inspired  us  to  investigate  related  problems  such  as 
curvature  estimation,  boundary  segmentation,  and  sensor  fusion. 

In  chapter  5  we  have  proposed  control  algorithms  that  can  be  used  to 
set  up  periodic  formations  of  satellites  on  elliptic  and  circular  orbits.  The 
shape  space  formed  by  the  angular  momentum  vectors  and  Laplace  vectors 
is  appropriate  to  describe  satellite  formations.  The  control  laws  we  propose 
are  based  on  a  Lyapunov  function  on  this  shape  space  and  proved  to  be 
convergent.  We  have  not  considered  the  effect  of  perturbations  such  as  J2 
effect.  This  is  currently  being  investigated. 
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